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Series Preface 



Mechanical engineering, an engineering discipline borne of the needs of the 
industrial revolution, is once again asked to do its substantial share in the 
call for industrial renewal. The general call is urgent as we face profound is- 
sues of productivity and competitiveness that require engineering solutions, 
among others. The Mechanical Engineering Series features graduate texts 
and research monographs intended to address the need for information in 
contemporary areas of mechanical engineering. 

The series is conceived as a comprehensive one that covers a broad range 
of concentrations important to mechanical engineering graduate education 
and research. We are fortunate to have a distinguished roster of consult- 
ing editors on the advisory board, each an expert in one of the areas of 
concentration. The names of the consulting editors are listed on the facing 
page of this volume. The areas of concentration are: applied mechanics; 
biomechanics; computational mechanics; dynamic systems and control; en- 
ergetics; mechanics of materials; processing; production systems; thermal 
science; and tribology. 
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Preface to the Second Edition 



The theory, methods and algorithms behind the development of robotic 
mechanical systems continue developing at a rate faster than they can be 
recorded. The second edition of Fundamentals of Robotic Mechanical Sys- 
tems does not claim a comprehensive account of developments up-to-date. 
Nevertheless, an attempt has been made to update the most impacting 
developments in these activities. Since the appearance of the first edition, 
many milestones can be cited. Advances in a host of applications areas can 
be mentioned, e.g., laparoscopy, haptics, and manufacturing, to mention a 
representative sample. 

Perhaps the most impressive achievements to be cited lie in the realm of 
space exploration. Indeed, in the period of interest we have seen the suc- 
cessful landing of the Sojourner on Mars, with the wheeled robot Pathfinder 
roaming on the Martian landscape in 1997. Along the same lines, the in- 
frastructure of the International Space Station was set in orbit in 2000, 
with the installation of Canadarm2, the successor of Canadarrn, following 
suit in 2001. Not less impressive are the achievements recorded on the the- 
oretical side of the areas of interest, although these have received much less 
media attention. To cite just one such accomplishment, one open question 
mentioned in the first edition was definitely closed in 1998 with a paper pre- 
sented at the International Workshop on Advances in Robot Kinematics. 
This question pertains to the 40th-degree polynomial derived by Husty — 
as reported in 1996 in a paper in Mechanism and Machine Theory — and 
allowing the computation of all forward-kinematics solutions of a general 
Stewart-Gough platform. Dietmaier reported an algorithm in that work- 
shop that is capable of generating a set of geometric parameters of the 
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platform that indeed lead to 40 real solutions. The conclusion then is that 
Husty’s polynomial is indeed minimal. 

In producing the Second Edition, we took the opportunity to clear the 
manuscript of errors and inaccuracies. An in-depth revision was conducted 
in-between. Special thanks go to Dr. Kourosh Etemadi Zanganeh, Can- 
met (Nepean, Ontario, Canada), for his invaluable help in the rewrit- 
ing of Chapter 8. Profs. Carlos Lopez-Cajun, Universidad Autonoma de 
Queretaro (Mexico), and J. Jesus Cervantes-Sanchez, Universidad de Gua- 
najuato (Mexico) pointed out many inconsistencies in the first edition. 
Moreover, Dr. Zheng Liu, Canadian Space Agency, St. -Hubert (Quebec, 
Canada), who is teaching a course based on the first six chapters of the 
book at McGill University, pointed out mistakes and gave valuable sugges- 
tions for improving the readability of the book. All these suggestions were 
incorporated in the Second Edition as suggested, except for one: While 
Dr. Liu suggested to expand on the use of Euler angles in Chapter 2, be- 
cause of their appeal to robotics engineers in industry, we decided to add, 
instead, a couple of exercises to the list corresponding to this chapter. The 
reason is that, in the author’s personal opinion, Euler angles are a neces- 
sary evil. Not being frame-invariant, their manipulation tends to become 
extremely cumbersome, as illustrated with those examples. Euler angles 
may be good for visualizing rigid-body rotations, but they are very bad 
at solving problems associated with these rotations using a computer or 
simple longhand calculations. Needless to say, the feedback received from 
students throughout over 15 years of using this material in the classroom, 
is highly acknowledged. 

One word of caution is in order: RVS, the software system used to vi- 
sualize robot motions and highlighted in the first edition, has not received 
either maintenance or updating. It still runs on SGI machines, but we have 
no plans for its porting into Windows. 

Since there is always room for improvement, we welcome suggestions from 
our readership. Please address these to the author, to the e-mail address 
included below. Updates on the book will be posted at 

WWW . cim . mcgill . ca/~rmsl 

The Solutions Manual has been expanded, to include more solutions of 
sampled problems. By the same token, the number of exercises at the end of 
the book has been expanded. The manual is typeset in DTf^X with Autocad 
drawings; it is available upon request from the publisher. 

Last, but by no means least, thanks are due to Dr. Svetlana Ostrovskaya, 
a Postdoctoral Fellow at McGill University, for her help with Chapter 10 
and the editing of the Second Edition. 



Montreal, January 2002 



Jorge Angeles 

angelesQcim . mcgill . ca 




Preface to the First Edition 



No todos los pensamientos son algoritmicos. 

— Mario Bunge^ 



The beginnings of modern robotics can be traced back to the late sixties 
with the advent of the microprocessor, which made possible the computer 
control of a multiaxial manipulator. Since those days, robotics has evolved 
from a technology developed around this class of manipulators for the re- 
playing of a preprogrammed task to a multidiscipline encompassing many 
branches of science and engineering. Research areas such as computer vi- 
sion, artificial intelligence, and speech recognition play key roles in the 
development and implementation of robotics; these are, in turn, multidis- 
ciplines supported by computer science, electronics, and control, at their 
very foundations. Thus we see that robotics covers a rather broad spec- 
trum of knowledge, the scope of this book being only a narrow band of this 
spectrum, as outlined below. 

Contemporary robotics aims at the design, control, and implementation 



^ Not all thinking processes are algorithmic — translation of the author — 
personal communication during the Symposium on the Brain-Mind Problem,. A 
Tribute to Professor Mario Bunge on His 75th Birthday, Montreal, September 
30, 1994. 
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of systems capable of performing a task defined at a high level, in a lan- 
guage resembling those used by humans to communicate among themselves. 
Moreover, robotic systems can take on forms of all kinds, ranging from the 
most intangible, such as interpreting images collected by a space sound, to 
the most concrete, such as cutting tissue in a surgical operation. We can, 
therefore, notice that motion is not essential to a robotic system, for this 
system is meant to replace humans in many of their activities, moving being 
but one of them. However, since robots evolved from early programmable 
manipulators, one tends to identify robots with motion and manipulation. 
Certainly, robots may rely on a mechanical system to perform their in- 
tended tasks. When this is the case, we can speak of robotic mechanical 
systems, which are the subject of this book. These tasks, in turn, can be 
of a most varied nature, mainly involving motions such as manipulation, 
but they can also involve locomotion. Moreover, manipulation can be as 
simple as displacing objects from a belt conveyor to a magazine. On the 
other hand, manipulation can also be as complex as displacing these objects 
while observing constraints on both motion and force, e.g., when cutting 
live tissue of vital organs. We can, thus, distinguish between plain manipu- 
lation and dextrous manipulation. Furthermore, manipulation can involve 
locomotion as well. 

The task of a robotic mechanical system is, hence, intimately related 
to motion control, which warrants a detailed study of mechanical systems 
as elements of a robotic system. The aim of this book can, therefore, be 
stated as establishing the foundations on which the design, control, and 
implementation of robotic mechanical systems are based. 

The book evolved from sets of lecture notes developed at McGill Uni- 
versity over the last twelve years, while I was teaching a two-semester se- 
quence of courses on robotic mechanical systems. For this reason, the book 
comprises two parts — an introductory and an intermediate part on robotic 
mechanical systems. Advanced topics, such as redundant manipulators, ma- 
nipulators with flexible links and joints, and force control, are omitted. The 
feedback control of robotic mechanical systems is also omitted, although 
the book refers the reader, when appropriate, to the specialized literature. 
An aim of the book is to serve as a textbook in a one- year robotics course; 
another aim is to serve as a reference to the practicing engineer. 

The book assumes some familiarity with the mathematics taught in any 
engineering or science curriculum in the first two years of college. Familiar- 
ity with elementary mechanics is helpful, but not essential, for the elements 
of this science needed to understand the mechanics of robotic systems are 
covered in the first three chapters, thereby making the book self-contained. 
These three chapters, moreover, are meant to introduce the reader to the 
notation and the basics of mathematics and rigid-body mechanics needed 
in the study of the systems at hand. The material covered in the same 
chapters can thus serve as reading material for a course on the mathemat- 
ics of robotics, intended for sophomore students of science and engineering. 
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prior to a more formal course on robotics. 

The first chapter is intended to give the reader an overview of the subject 
matter and to highlight the major issues in the realm of robotic mechanical 
systems. Chapter 2 is devoted to notation, nomenclature, and the basics of 
linear transformations to understand best the essence of rigid-body kine- 
matics, an area that is covered in great detail throughout the book. A 
unique feature of this chapter is the discussion of the hand-eye calibration 
problem: Many a paper has been written in an attempt to solve this fun- 
damental problem, always leading to a cumbersome solution that invokes 
nonlinear-equation solving, a task that invariably calls for an iterative pro- 
cedure; moreover, within each iteration, a singular- value decomposition, 
itself iterative as well, is required. In Chapter 2, a novel approach is in- 
troduced, which resorts to invariant properties of rotations and leads to a 
direct solution, involving straightforward matrix and vector multiplications. 
Chapter 3 reviews, in turn, the basic theorems of rigid-body kinetostatics 
and dynamics. The viewpoint here represents a major departure from most 
existing books on robotic manipulators: proper orthogonal matrices can be 
regarded as coordinate transformations indeed, but they can also be re- 
garded as representations, once a coordinate frame has been selected, of 
rigid-body rotations. I adopt the latter viewpoint, and hence, fundamental 
concepts are explained in terms of their invariant properties, i.e., proper- 
ties that are independent of the coordinate frame adopted. Hence, matrices 
are used first and foremost to represent the physical motions undergone by 
rigid bodies and systems thereof; they are to be interpreted as such when 
studying the basics of rigid-body mechanics in this chapter. Chapter 4 is 
the first chapter entirely devoted to robotic mechanical systems, properly 
speaking. This chapter covers extensively the kinematics of robotic ma- 
nipulators of the serial type. However, as far as displacement analysis is 
concerned, the chapter limits itself to the simplest robotic manipulators, 
namely, those with a decoupled architecture, i.e., those that can be decom- 
posed into a regional architecture for the positioning of one point of their 
end-effector (EE), and a local architecture for the orientation of their EE. 
In this chapter, the notation of Denavit and Hartenberg is introduced and 
applied consistently throughout the book. Jacobian matrices, workspaces, 
singularities, and kinetostatic performance indices are concepts studied in 
this chapter. A novel algorithm is included for the determination of the 
workspace boundary of positioning manipulators. Furthermore, Chapter 5 
is devoted to the topic of trajectory planning, while limiting its scope to 
problems suitable to a first course on robotics; this chapter thus focuses on 
pick-and-place operations. Chapter 6, moreover, introduces the dynamics 
of robotic manipulators of the serial type, while discussing extensively the 
recursive Newton-Euler algorithm and laying the foundations of multibody 
dynamics, with an introduction to the Euler-Lagrange formulation. The 
latter is used to derive the general algebraic structure of the mathematical 
models of the systems under study, thus completing the introductory part 




XIV 



Preface to the First Edition 



of the book. 

The intermediate part comprises four chapters. Chapter 7 is devoted to 
the increasingly important problem of determining the angular velocity and 
the angular acceleration of a rigid body, when the velocity and acceleration 
of a set of its points are known. Moreover, given the intermediate level of 
the chapter, only the theoretical aspects of the problem are studied, and 
hence, perfect measurements of point position, velocity, and acceleration 
are assumed, thereby laying the foundations for the study of the same 
problems in the presence of noisy measurements. This problem is finding 
applications in the control of parallel manipulators, which is the reason 
why it is included here. If time constraints so dictate, this chapter can be 
omitted, for it is not needed in the balance of the book. 

The formulation of the inverse kinematics of the most general robotic ma- 
nipulator of the serial type, leading to a univariate polynomial of the 16th 
degree, not discussed in previous books on robotics, is included in Chap- 
ter 8. Likewise, the direct kinematics of the platform manipulator popularly 
known as the Stewart platform, a.k.a. the Stew art- Gough platform, leading 
to a 16th-degree monovariate polynomial, is also given due attention in this 
chapter. Moreover, an alternative approach to the monovariate-polynomial 
solution of the two foregoing problems, that is aimed at solving them semi- 
graphically, is introduced in this chapter. With this approach, the under- 
lying multivariate algebraic system of equations is reduced to a system of 
two nonlinear bivariate equations that are trigonometric rather than poly- 
nomial. Each of these two equations, then, leads to a contour in the plane 
of the two variables, the desired solutions being found as the coordinates 
of the intersections of the two contours. 

Discussed in Chapter 9 is the problem of trajectory planning as per- 
taining to continuous paths, which calls for some concepts of differential 
geometry, namely, the Ffenet-Serret equations relating the tangent, nor- 
mal, and binormal vectors of a smooth curve to their rates of change with 
respect to the arc length. The chapter relies on cubic parametric splines 
for the synthesis of the generated trajectories in joint space, starting from 
their descriptions in Cartesian space. Finally, Chapter 10 completes the 
discussion initiated in Chapter 6, with an outline of the dynamics of paral- 
lel manipulators and rolling robots. Here, a multibody dynamics approach 
is introduced, as in the foregoing chapter, that eases the formulation of the 
underlying mathematical models. 

Two appendices are included: Appendix A summarizes a series of facts 
from the kinematics of rotations, that are available elsewhere, with the 
purpose of rendering the book self-contained; Appendix B is devoted to the 
numerical solution of over- and underdetermined linear algebraic systems, 
its purpose being to guide the reader to the existing robust techniques for 
the computation of least-square and minimum-norm solutions. The book 
concludes with a set of problems, along with a list of references, for all ten 
chapters. 
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On Notation 

The important issue of notation is given due attention. In figuring out the 
notation, I have adopted what I call the norm. Under this norm, the 
notation should be 

1. Comprehensive, 

2. Concise, and 

3. Consistent. 

Within this norm, I have used boldface fonts to indicate vectors and 
matrices, with uppercases reserved for matrices and lowercases for vectors. 
In compliance with the invariant approach adopted at the outset, I do not 
regard vectors solely as arrays, but as geometric or mechanical objects. 
Regarding such objects as arrays is necessary only when it is required to 
perform operations with them for a specific purpose. An essential feature 
of vectors in a discussion is their dimension, which is indicated with a 
single number, as opposed to the convention whereby vectors are regarded 
as matrix arrays of numbers; in this convention, the dimension has to be 
indicated with two numbers, one for the number of columns, and one for the 
number of rows; in the case of vectors, the latter is always one, and hence, 
need not be mentioned. Additionally, calligraphic literals are reserved for 
sets of points or of other objects. Since variables are defined every time that 
they are introduced, and the same variable is used in the book to denote 
different concepts in different contexts, a list of symbols is not included. 



How to Use the Book 

The book can be used as a reference or as a text for the teaching of the 
mechanics of robots to an audience that ranges from junior undergraduates 
to doctoral students. In an introductory course, the instructor may have 
to make choices regarding what material to skip, given that the duration 
of a regular semester does not allow to cover all that is included in the 
first six chapters. Topics that can be skipped, if time so dictates, are the 
discussions, in Chapter 4, of workspaces and performance indices, and the 
section on simulation in Chapter 6. Under strict time constraints, the whole 
Chapter 5 can be skipped, but then, the instructor will have to refrain 
from assigning problems or projects that include calculating the inverse 
dynamics of a robot performing pick-and-place operations. None of these 
has been included in Section 6 of the Exercises. 

If sections of Chapters 4 and 5 have been omitted in a first course, it is 
highly advisable to include them in a second course, prior to discussing the 
chapters included in the intermediate part of the book. 
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An Overview of Robotic Mechanical 
Systems 



1.1 Introduction 

In defining the scope of our subject, we have to establish the genealogy of 
robotic mechanical systems. These are, obviously, a subclass of the much 
broader class of mechanical systems. Mechanical systems, in turn, consti- 
tute a subset of the more general concept of dynamic systems. Therefore, 
in the final analysis, we must have an idea of what, in general, a system is. 

The Concise Oxford Dictionary defines system as a “complex whole, set 
of connected things or parts, organized body of material or immaterial 
things,” whereas the Random House College Dictionary defines the same 
as “an assemblage or combination of things or parts forming a complex 
or unitary whole.” Le Petit Robert, in turn, defines system as “Ensem- 
ble possedant une structure, constituant un tout organique,” which can 
be loosely translated as “A structured assemblage constituting an organic 
whole.” In the foregoing definitions, we note that the underlying idea is 
that of a set of elements interacting as a whole. 

On the other hand, a dynamic system is a subset of the set of systems. 
For our purposes, we can dispense with a rigorous definition of this concept. 
Suffice it to say that a d}mamic system is a system in which one can distin- 
guish three elements, namely, a state, an input, and an output, in addition 
to a rule of transition from one current state to a future one. Moreover, 
the state is a functional of the input and a function of a previous state. In 




2 



1. An Overview of Robotic Mechanical Systems 



this concept, then, the idea of order is important, and can be taken into 
account by properly associating each state value with time. The state at 
every instant is a functional, as opposed to a function, of the input, which is 
characteristic of d}mamic systems. This means that the state of a dynamic 
system at a certain instant is determined not only by the value of the input 
at that instant, but also by the past history of that input. By virtue of this 
property, dynamic systems are said to have memory. 

On the contrary, systems whose state at a given instant is only a function 
of the input at the current time are static and are said to have no memory. 
Additionally, since the state of a d}mamic system is a result of all the past 
history of the input, the future values of this having no influence on the 
state, dynamic systems are said to be nonanticipative or causal. By the 
same token, systems whose state is the result of future values of the input 
are said to be anticipative or noncausal. In fact, we will not need to worry 
about the latter, and hence, all systems we will study can be assumed to 
be causal. 

Obviously, a mechanical system is a system composed of mechanical ele- 
ments. If this system complies with the definition of dynamic system, then 
we end up with a dynamic mechanical system. For brevity, we will refer to 
such systems as mechanical systems, the dynamic property being taken for 
granted throughout the book. Mechanical systems of this type are those 
that occur whenever the inertia of their elements is accounted for. Static 
mechanical systems are those in which inertia is neglected. Moreover, the 
elements constituting a mechanical system are rigid and deformable solids, 
compressible and incompressible fluids, and inviscid and viscous fluids. 

Prom the foregoing discussion, then, it is apparent that mechanical sys- 
tems can be constituted either by lumped-parameter or by distributed- 
parameter elements. The former reduce to particles; rigid bodies; massless, 
conservative springs; and massless, nonconservative dashpots. The latter 
appear whenever bodies are modeled as continuous media. In this book, we 
will focus on lumped-parameter mechanical systems. 

Furthermore, a mechanical system can be either natural or man-made, 
the latter being the subject of our study. Man-made mechanical systems 
can be either controlled or uncontrolled. Most engineering systems are con- 
trolled mechanical systems, and hence, we will focus on these. Moreover, 
a controlled mechanical system may be robotic or nonrobotic. The lat- 
ter are systems supplied with primitive controllers, mostly analog, such 
as thermostats, servovalves, etc. Robotic mechanical systems, in turn, can 
be programmable, such as most current industrial robots, or intelligent, 
as discussed below. Programmable mechanical systems obey motion com- 
mands either stored in a memory device or generated on-line. In either 
case, they need primitive sensors, such as joint encoders, accelerometers, 
and dynamometers. 

Intelligent robots or, more broadly speaking, intelligent machines, are 
yet to be demonstrated, but have become the focus of intensive research. 




1.2 The General Structure of Robotic Mechanical Systems 3 



If intelligent machines are ever feasible, they will depend highly on a so- 
phisticated sensory system and the associated hardware and software for 
the processing of the information supplied by the sensors. The processed 
information would then be supplied to the actuators in charge of producing 
the desired motion of the robot. Contrary to programmable robots, whose 
operation is limited to structured environments, intelligent machines should 
be capable of reacting to unpredictable changes in an unstructured environ- 
ment. Thus, intelligent machines should be supplied with decision-making 
capabilities aimed at mimicking the natural decision-making process of liv- 
ing organisms. This is the reason why such systems are termed intelligent 
in the first place. Thus, intelligent machines are expected to perceive their 
environment and draw conclusions based on this perception. What is sup- 
posed to make these systems intelligent is their capability of perceiving, 
which involves a certain element of subjectivity. By far, the most complex 
of perception tasks, both in humans and machines, is visual (Levine, 1985; 
Horn, 1986). 

In summary, then, an intelligent machine is expected to (*) perceive the 
environment; (m) reason about the perceived information; {in) make deci- 
sions based on this perception; and {iv) act according to a plan specified at 
a very high level. What the latter means is that the motions undergone by 
the machine are decided upon based on instructions similar to those given 
to a human being, like bring me a glass of water without spilling the water. 

Whether intelligent machines with all the above features will be one day 
possible or not is still a subject of discussion, sometimes at a philosophical 
level. Penrose (1994) wrote a detailed discussion refuting the claim that 
intelligent machines are possible. 

A genealogy of mechanical systems, including robotic ones, is given in 
Fig. 1.1. In that figure, we have drawn a dashed line between mechanical 
systems and other systems, both man-made and natural, in order to em- 
phasize the interaction of mechanical systems with electrical, thermal, and 
other systems, including the human system, which is present in telemanip- 
ulators, to be discussed below. 



1.2 The General Structure of Robotic Mechanical 
Systems 

From Section 1.1, then, a robotic mechanical system is composed of a few 
subsystems, namely, (*) a mechanical subsystem composed in turn of both 
rigid and deformable bodies, although the systems we will study here are 
composed only of the former; (m) a sensing subsystem; {in) an actuation 
subsystem; {iv) a controller; and {v) an information-processing subsystem. 
Additionally, these subsystems communicate among themselves via inter- 
faces, whose function consists basically of decoding the transmitted infor- 
mation from one medium to another. Figure 1.2 shows a block diagram 
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FIGURE 1.1. A genealogy of robotic mechanical systems. 

representation of a typical robotic mechanical system. Its input is a pre- 
scribed task, which is defined either on the spot or off-line. The former case 
is essential for a machine to be called intelligent, while the latter is present 
in programmable machines. Thus, tasks would be described to intelligent 
machines by a software system based on techniques of artificial intelligence 
(AI). This system would replace the human being in the decision-making 
process. Programmable robots require human intervention either for the 
coding of preprogrammed tasks at a very low level or for telemanipulation. 
A very low level of programming means that the motions of the machine are 
specified as a sequence of either joint motions or Cartesian coordinates as- 
sociated with landmark points of that specific body performing the task at 
hand. The output of a robotic mechanical system is the actual task, which 
is monitored by the sensors. The sensors, in turn, transmit task information 
in the form of feedback signals, to be compared with the prescribed task. 
The errors between the prescribed and the actual task are then fed back 
into the controller, which then synthesizes the necessary corrective signals. 
These are, in turn, fed back into the actuators, which then drive the me- 
chanical system through the required task, thereby closing the loop. The 
problem of robot control has received extensive attention in the literature, 
and will not be pursued here. The interested reader is referred to the ex- 
cellent works on the subject, e.g., those of Samson, Le Borgne, and Espiau 
(1991) and, at a more introductory level, of Spong and Vidyasagar (1989). 
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SJV: synthesized joint variables (angles and torques) 

P&FS: position and force signals 

C&JS: Cartesian and joint signals 

AJV: actual joint variables (angles and torques) 



FIGURE 1.2. Block diagram of a general robotic mechanical system. 



Of special relevance to robot control is the subject of nonlinear control at 
large, a pioneer here being Isidori (1989). 

Robotic mechanical systems with a human being in their control loop 
are called telemanipulators. Thus, a telemanipulator is a robotic mechan- 
ical system in which the task is controlled by a human, possibly aided 
by sophisticated sensors and display units. The human operator is then a 
central element in the block diagram loop of Fig. 1.2. Based on the infor- 
mation displayed, the operator makes decisions about corrections in order 
to accomplish the prescribed task. Shown in Fig. 1.3 is a telemanipula- 
tor to be used in space applications, namely, the Canadarm2, along with 
the Special-Purpose Dextrous Manipulator (SPDM), both mounted on the 
Mobile Servicing System (MSS). Moreover, a detailed view of the Special- 
Purpose Dextrous Manipulator is shown in Fig. 1.4. In the manipulators 
of these two figures, the human operator is an astronaut who commands 
and monitors the motions of the robot from inside the EVA (extravehicular 
activity) workstation. The number of controlled axes of each of these ma- 
nipulators being larger than six, both are termed redundant. The challenge 
here is that the mapping from task coordinates to joint motions is not 
unique, and hence, among the infinitely many joint trajectories that the 
operator has at his or her disposal for a given task, an on-board processor 
must evaluate the best one according to a performance criterion. 

While the manipulators of Figs. 1.3 and 1.4 are still at the development 
stage, examples of robotic mechanical systems in operation are the well- 
known six-axis industrial manipulators, six-degree-of-freedom flight simu- 
lators, walking machines, mechanical hands, and rolling robots. We outline 
the various features of these systems below. 
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FIGURE 1.3. Canadarm2 and Special-Purpose Dextrous Manipulator (courtesy 
of the Canadian Space Agency.) 

1.3 Serial Manipulators 

Among all robotic mechanical systems mentioned above, robotic manipu- 
lators deserve special attention, for various reasons. One is their relevance 
in industry. Another is that they constitute the simplest of all robotic me- 
chanical systems, and hence, appear as constituents of other, more complex 
robotic mechanical systems, as will become apparent in later chapters. A 
manipulator, in general, is a mechanical system aimed at manipulating ob- 
jects. Manipulating, in turn, means to move something with one’s hands, 
as it derives from the Latin manus, meaning hand. The basic idea behind 
the foregoing concept is that hands are among the organs that the human 
brain can control mechanically with the highest accuracy, as the work of 
an artist like Picasso, of an accomplished guitar player, or of a surgeon can 
attest. 

Hence, a manipulator is any device that helps man perform a manip- 
ulating task. Although manipulators have existed ever since man created 
the first tool, only very recently, namely, by the end of World War II, have 
manipulators developed to the extent that they are now capable of actu- 
ally mimicking motions of the human arm. In fact, during WWII, the need 
arose for manipulating probe tubes containing radioactive substances. This 
led to the first six-degree-of- freedom (DOF) manipulators. 

Shortly thereafter, the need for manufacturing workpieces with high ac- 
curacy arose in the aircraft industry, which led to the first numerically- 
controlled (NC) machine tools. The synthesis of the six-DOF manipulator 
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FIGURE 1.4. Special-Purpose Dextrous Manipulator (courtesy of the Canadian 
Space Agency.) 

and the NC machine tool produced what became the robotic manipula- 
tor. Thus, the essential difference between the early manipulator and the 
evolved robotic manipulator is the term robotic, which has only recently, 
as of the late sixties, come into the picture. A robotic manipulator is to 
be distinguished from the early manipulator by its capability of lending 
itself to computer control. Whereas the early manipulator needed the pres- 
ence of a manned master manipulator, the robotic manipulator can be pro- 
grammed once and for all to repeat the same task forever. Programmable 
manipulators have existed for about 30 years, namely, since the advent of 
microprocessors, which allowed a human master to teach the manipulator 
by actually driving the manipulator itself, or a replica thereof, through a 
desired task, while recording all motions undergone by the master. Thus, 
the manipulator would later repeat the identical task by mere playback. 
However, the capabilities of industrial robots are fully exploited only if the 
manipulator is programmed with software, rather than actually driving it 
through its task trajectory, which many a time, e.g., in car-body spot- 
welding, requires separating the robot from the production line for more 
than a week. One of the objectives of this book is to develop tools for the 
programming of robotic manipulators. 

However, the capabilities offered by robotic mechanical systems go well 
beyond the mere playback of preprogrammed tasks. Current research aims 
at providing robotic systems with software and hardware that will allow 
them to make decisions on the spot and learn while performing a task. The 
implementation of such systems calls for task-planning techniques that fall 
beyond the scope of this book and, hence, will not be treated here. For a 
glimpse of such techniques, the reader is referred to the work of Latombe 
(1991) and the references therein. 
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FIGURE 1.5. A six-degree-of-freedom flight simulator (courtesy of CAE Elec- 
tronics Ltd.) 

1.4 Parallel Manipulators 

Robotic manipulators first appeared as mechanical systems constituted by 
a structure consisting of very robust links coupled by either rotational or 
translating joints, the former being called revolutes, the latter prismatic 
joints. Moreover, these structures are a concatenation of links, thereby 
forming an open kinematic chain, with each link coupled to a predeces- 
sor and a successor, except for the two end links, which are coupled only 
to either a predecessor or to a successor, but not to both. Because of the 
serial nature of the coupling of links in this type of manipulator, even 
though they are supplied with structurally robust links, their load-carrying 
capacity and their stiffness is too low when compared with the same prop- 
erties in other multiaxis machines, such as NC machine tools. Obviously, a 
low stiffness implies a low positioning accuracy. In order to remedy these 
drawbacks, parallel manipulators have been proposed to withstand higher 
payloads with lighter links. In a parallel manipulator, we distinguish one 
base platform, one moving platform, and various legs. Each leg is, in turn, 
a kinematic chain of the serial type, whose end links are the two platforms. 
Contrary to serial manipulators, all of whose joints are actuated, parallel 
manipulators contain unactuated joints, which brings about a substantial 
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difference between the two types. The presence of unactuated joints makes 
the analysis of parallel manipulators, in general, more complex than that 
of their serial counterparts. 

A paradigm of parallel manipulators is the flight simulator, consisting of 
six legs actuated by hydraulic pistons, as displayed in Fig. 1.5. Recently, an 
explosion of novel designs of parallel manipulators has occurred aimed at 
fast assembly operations, namely, the Delta robot (Clavel, 1988), developed 
at the Lausanne Federal Polytechnic Institute, shown in Fig. 1.6; the Hexa 
robot (Pierrot et ah, 1991), developed at the University of Montpellier; 
and the Star robot (Herve and Sparacino, 1992), developed at the Ecole 
Centrale of Paris. One more example of parallel manipulator is the Truss- 
arm, developed at the University of Toronto Institute of Aerospace Studies 
(UTIAS), shown in Fig. 1.7a (Hughes et ah, 1991). Merlet (2000), of the 
Institut National de Recherche era Informatique et era Autornatique, Sophia- 
Antipolis, France, developed a six-axis parallel robot, called in French a 
main gauche, or left hand, shown in Fig. 1.7b, to be used as an aid to an- 
other robot, possibly of the serial type, to enhance its dexterity. Hayward, 
of McGill University, designed and constructed a parallel manipulator to 
be used as a shoulder module for orientation tasks (Hayward, 1994); the 
module is meant for three-degree-of-freedom motions, but is provided with 
four hydraulic actuators, which gives it redundant actuation — Fig. 1.7c. 




FIGURE 1.6. The Clavel Delta robot. 
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(c) 



FIGURE 1.7. A sample of parallel manipulators: (a) The UTIAS Trussarm (cour- 
tesy of Prof P. C. Hughes); (b) the Merlet left hand (courtesy of Dr. J.-P. Merlet); 
and (c) the Hayward shoulder module (courtesy of Prof V. Hayward.) 
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1.5 Robotic Hands 

As stated above, the hand can be regarded as the most complex mechanical 
subsystem of the human manipulation system. Other mechanical subsys- 
tems constituting this system are the arm and the forearm. Moreover, the 
shoulder, coupling the arm with the torso, can be regarded as a spherical 
joint, i.e., the concatenation of three revolute joints with intersecting axes. 
Furthermore, the arm and the forearm are coupled via the elbow, with the 
forearm and the hand finally being coupled by the wrist. Frequently, the 
wrist is modeled as a spherical join as well, while the elbow is modeled as a 
simple revolute joint. Robotic mechanical systems mimicking the motions 
of the arm and the forearm constitute the manipulators discussed in the 
previous section. Here we outline more sophisticated manipulation systems 
that aim at producing the motions of the human hand, i.e., robotic me- 
chanical hands. These robotic systems are meant to perform manipulation 
tasks, a distinction being made between simple manipulation and dextrous 
manipulation. What the former means is the simplest form, in which the 
fingers play a minor role, namely, by serving as simple static structures that 
keep an object rigidly attached with respect to the palm of the hand — when 
the palm is regarded as a rigid body. As opposed to simple manipulation, 
dextrous manipulation involves a controlled motion of the grasped object 
with respect to the palm. Simple manipulation can be achieved with the 
aid of a manipulator and a gripper, and need not be further discussed here. 
The discussion here is about dextrous manipulation. 

In dextrous manipulation, the grasped object is required to move with re- 
spect to the palm of the grasping hand. This kind of manipulation appears 
in performing tasks that require high levels of accuracy, like handwriting 
or cutting tissue with a scalpel. Usually, grasping hands are multifingered, 
although some grasping devices exist that are constituted by a simple, 
open, highly redundant kinematic chain (Pettinato and Stephanou, 1989). 
The kinematics of grasping is discussed in Chapter 4. The basic kinematic 
structure of a multifingered hand consists of a palm, which plays the role 
of the base of a simple manipulator, and a set of fingers. Thus, kinemat- 
ically speaking, a multifingered hand has a tree topology, i.e., it entails a 
common rigid body, the palm, and a set of jointed bodies emanating from 
the palm. Upon grasping an object with all the fingers, the chain becomes 
closed with multiple loops. Moreover, the architecture of the fingers is that 
of a simple manipulator. It consists of a number — two to four — of revolute- 
coupled links playing the role of phalanges. However, unlike manipulators 
of the serial type, whose joints are all independently actuated, those of a 
mechanical finger are not and, in many instances, are driven by one single 
master actuator, the remaining joints acting as slaves. Many versions of 
multifingered hands exist: Stanford/JPL; Utah/MIT; TU Munich; Karls- 
ruhe; Bologna; Leuven; Milan; Belgrade; and University of Toronto, among 
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FIGURE 1.8. The four-fingered hydraulically actuated TU Munich Hand (cour- 
tesy of Prof F. Pfeiffer.) 

others. Of these, the Utah/MIT Hand (Jacobsen et ah, 1984; 1986) is com- 
mercially available. It consists of four fingers, one of which is opposed to 
the other three and hence, plays the role of the human thumb. Each finger 
consists, in turn, of four phalanges coupled by revolute joints; each of these 
is driven by two tendons that can deliver force only when in tension, each 
being actuated independently. The TU Munich Hand, shown in Fig. 1.8, 
is designed with four identical fingers laid out symmetrically on a hand 
palm. This hand is hydraulically actuated, and provided with a very high 
payload-to- weight ratio. Indeed, each finger weighs only 1.470 N, but can 
exert a force of up to 30 N. 

We outline below some problems and research trends in the area of dex- 
trous hands. A key issue here is the programming of the motions of the 
fingers, which is a much more complicated task than the programming 
of a six-axis manipulator. In this regard, Liu et al. (1989) introduced a 
task-analysis approach meant to program robotic hand motions at a higher 
level. They use a heuristic, knowledge-based approach. From an analysis 
of the various modes of grasping, they conclude that the requirements for 
grasping tasks are (*) stability, (m) manipulability, {in) torquability, and 
{iv) radial rotatability. Stability is defined as a measure of the tendency 
of an object to return to its original position after disturbances. Manipu- 
lability, as understood in this context, is the ability to impart motion to 
the object while keeping the fingers in contact with the object. Torquabi- 
lity, or tangential rotatability, is the ability to rotate the long axis of an 
object — here the authors must assume that the manipulated objects are 
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convex and can be approximated by three-axis ellipsoids, thereby distin- 
guishing between a longest and a shortest axis — with a minimum force, for 
a prescribed amount of torque. Finally, radial rotatability is the ability to 
rotate the grasped object about its long axis with minimum torque about 
the axis. 

Furthermore, Allen et al. (1989) introduced an integrated system of both 
hardware and software for dextrous manipulation. The system consists 
of a Sun-3 workstation controlling a Puma 500 arm with VAL-IL The 
Utah/MIT hand is mounted on the end-effector of the arm. The system in- 
tegrates force and position sensors with control commands for both the arm 
and the hand. To demonstrate the effectiveness of their system, the authors 
implemented a task consisting of removing a light bulb from its socket. Fi- 
nally, Rus (1992) reports a paradigm allowing the high-level, task-oriented 
manipulation control of planar hands. Whereas technological aspects of 
dextrous manipulation are highly advanced, theoretical aspects are still 
under research in this area. An extensive literature survey, with 405 refer- 
ences on the subject of manipulation, is given by Reynaerts (1995). 



1.6 Walking Machines 

We focus here on multilegged walking devices, i.e., machines with more 
than two legs. In walking machines, stability is the main issue. One distin- 
guishes between two types of stability, static and dynamic. Static stability 
refers to the ability of sustaining a configuration from reaction forces only, 
unlike dynamic stability, which refers to that ability from both reaction and 
inertia forces. Intuitively, it is apparent that static stability requires more 
contact points and, hence, more legs, than dynamic stability. Hopping de- 
vices (Raibert, 1986) and bipeds (Vukobratovic and Stepanenko, 1972) are 
examples of walking machines whose motions aredependent upon dynamic 
stability. For static balance, a walking machine requires a kinematic struc- 
ture capable of providing the ground reaction forces needed to balance the 
weight of the machine. A biped is not capable of static equilibrium because 
during the swing phase of one leg, the body is supported by a single con- 
tact point, which is incapable of producing the necessary balancing forces 
to keep it in equilibrium. For motion on a horizontal surface, a minimum 
of three legs is required to produce static stability. Indeed, with three legs, 
one of these can undergo swing while the remaining two legs are in contact 
with the ground, and hence, two contact points are present to provide the 
necessary balancing forces from the ground reactions. 

By the same token, the minimum number of legs required to sustain static 
stability in general is four, although a very common architecture of walking 
machines is the hexapod, examples of which are the Ohio State University 
(OSU) Hexapod (Klein et ah, 1983) and the OSU Adaptive Suspension 
Vehicle (ASV) (Song and Waldron, 1989), shown in Fig. 1.10. A six-legged 




14 



1. An Overview of Robotic Mechanical Systems 




FIGURE 1.9. A prototype of the TU Munich Hexapod (Courtesy of Prof. F. Pfeif- 
fer. Reproduced with permission of TSI Enterprises, Inc.) 

walking machine with a design that mimics the locomotion system of the 
Carausius rnorosus (Graham, 1972), also known as the walking stick, has 
been developed at the Technical University of Munich (Pfeiffer et ah, 1995). 
A prototype of this machine, known as the TUM Hexapod, is included in 
Fig. 1.9. The legs of the TUM Hexapod are operated under neural- network 
control, which gives them a reflexlike response when encountering obstacles. 
Upon sensing an obstacle, the leg bounces back and tries again to move 
forward, but raising the foot to a higher level. 

Other machines that are worth mentioning are the Sutherland, Sprout 
and Associates Hexapod (Sutherland and Ullner, 1984), the Titan series of 
quadrupeds (Hirose et al., 1985) and the Odetics series of axially symmetric 
hexapods (Russell, 1983). 

A survey of walking machines, of a rather historical interest by now, 
is given in (Todd, 1985), while a more recent comprehensive account of 
walking machines is available in a special issue of The International Journal 
of Robotics Research (Volume 9, No. 2). 

Walking machines appear as the sole means of providing locomotion in 
highly unstructured environments. In fact, the unique adaptive suspension 
provided by these machines allows them to navigate on uneven terrain. 
However, walking machines cannot navigate on every type of uneven ter- 
rain, for they are of limited dimensions. Hence, if terrain irregularities such 
as a crevasse wider than the maximum horizontal leg reach or a cliff of 
depth greater than the maximum vertical leg reach are present, then the 
machine is prevented from making any progress. This limitation, however, 
can be overcome by providing the machine with the capability of attaching 
its feet to the terrain in the same way as a mountain climber goes up a cliff. 
Moreover, machine functionality is limited not only by the topography of 
the terrain, but also by its constitution. Whereas hard rock poses no serious 
problem to a walking machine, muddy terrain can hamper its operation to 
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FIGURE 1.10. The OSU ASV. An example of a six-legged walking machine 
(courtesy of Prof K. Waldron. Reproduced with permission of The MIT Press.) 

the point that it may jam the machine. Still, under such adverse conditions, 
walking machines offer a better maneuverability than other vehicles. Some 
walking machines have been developed and are operational, but their op- 
eration is often limited to slow motions. It can be said, however, that like 
research on multifingered hands, the pace of theoretical research on walking 
machines has been much slower than that of their technological develop- 
ments. The above-mentioned OSU ASV and the TU Munich Hexapod are 
among the most technologically developed walking machines. 



1.7 Rolling Robots 

While parallel manipulators indeed solve many inherent problems of serial 
manipulators, their workspaces are more limited than those of the latter. As 
a matter of fact, even serial manipulators have limited workspaces due to 
the finite lengths of their links. Manipulators with limited workspaces can 
be enhanced by mounting them on rolling robots. These are systems evolved 
from earlier systems called automatic guided vehicles, or AGVs for short. 
AGVs in their most primitive versions are four-wheeled electrically powered 
vehicles that perform moving tasks with a certain degree of autonomy. 
However, these vehicles are usually limited to motions along predefined 
tracks that are either railways or magnetic strips glued to the ground. 

The most common rolling robots use conventional wheels, i.e., wheels 
consisting basically of a pneumatic tire mounted on a hub that rotates 
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about an axle fixed to the platform of the robot. Thus, the operation of 
these machines does not differ much from that of conventional terrestrial 
vehicles. An essential difference between rolling robots and other robotic 
mechanical systems is the kinematic constraints between wheel and ground 
in the former. These constraints are of a type known as nonholonomic, as 
discussed in detail in Chapter 6. Nonholonomic constraints are kinematic 
relations between point velocities and angular velocities that cannot be 
integrated in the form of algebraic relations between translational and ro- 
tational displacement variables. The outcome of this lack of integrability 
leads to a lack of a one-to-one relationship between Cartesian variables and 
joint variables. In fact, while angular displacements read by joint encoders 
of serial manipulators determine uniquely the position and orientation of 
their end-effector, the angular displacement of the wheels of rolling ma- 
chines do not determine the position and orientation of the vehicle body. 
As a matter of fact, the control of rolling robots bears common features 
with that of the redundancy resolution of manipulators of the serial type at 
the joint-rate level. In these manipulators, the number of actuated joints 
is greater than the dimension of the task space. As a consequence, the 
task velocity does not determine the joint rates. Not surprisingly, the two 
types of problems are being currently solved using the same tools, namely, 
differential geometry and Lie algebra (De Luca and Oriolo, 1995). 

As a means to supply rolling robots with 3-dof capabilities, omnidirec- 
tional wheels (ODW) have been proposed. An example of ODWs are those 
that bear the name of Mekanum wheels, consisting of a hub with rollers 
on its periphery that roll freely about their axes, the latter being oriented 
at a constant angle with respect to the hub axle. In Fig. 1.11, a Mekanum 
wheel is shown, along with a rolling robot supplied with this type of wheels. 
Rolling robots with ODWs are, thus, 3-dof vehicles, and hence, can trans- 
late freely in two horizontal directions and rotate independently about a 
vertical axis. However, like their 2-dof counterparts, 3-dof rolling robots 
are also nonholonomic devices, and thus, pose the same problems for their 
control as the former. 




(a) 



(b) 



FIGURE 1.11. (a) A Mekanum wheel; (b) rolling robot supplied with Mekanum 
wheels. 
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Recent developments in the technology of rolling robots have been re- 
ported that incorporate alternative types of ODWs. For example, Killough 
and Pin (1992) developed a rolling robot with what they call orthogonal 
ball wheels, consisting basically of spherical wheels that can rotate about 
two mutually orthogonal axes. West and Asada (1995), in turn, designed a 
rolling robot with ball wheels, i.e., balls that act as omnidirectional wheels; 
each ball being mounted on a set of rollers, one of which is actuated; hence, 
three such wheels are necessary to fully control the vehicle. The unactu- 
ated rollers serve two purposes, i.e., to provide stability to the wheels and 
the vehicle, and to measure the rotation of the ball, thereby detecting slip. 
Furthermore, Borenstein (1993) proposed a mobile robot with four degrees 
of freedom; these were achieved with two chassis coupled by an extensible 
link, each chassis being driven by two actuated conventional wheels. 




2 

Mathematical Background 



2.1 Preamble 

First and foremost, the study of motions undergone by robotic mechani- 
cal systems or, for that matter, by mechanical systems at large, requires 
a suitable motion representation. Now, the motion of mechanical systems 
involves the motion of the particular links comprising those systems, which 
in this book are supposed to be rigid. The assumption of rigidity, although 
limited in scope, still covers a wide spectrum of applications, while pro- 
viding insight into the motion of more complicated systems, such as those 
involving deformable bodies. 

The most general kind of rigid-body motion consists of both transla- 
tion and rotation. While the study of the former is covered in elementary 
mechanics courses and is reduced to the mechanics of particles, the latter 
is more challenging. Indeed, point translation can be studied simply with 
the aid of 3-dimensional vector calculus, while rigid-body rotations require 
the introduction of tensors, i.e., entities mapping vector spaces into vector 
spaces. 

Emphasis is placed on invariant concepts, i.e., items that do not change 
upon a change of coordinate frame. Examples of invariant concepts are ge- 
ometric quantities such as distances and angles between lines. Although we 
may resort to a coordinate frame and vector algebra to compute distances 
and angles and represent vectors in that frame, the final result will be inde- 
pendent of how we choose that frame. The same applies to quantities whose 
evaluation calls for the introduction of tensors. Here, we must distinguish 
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between the physical quantity represented by a vector or a tensor and the 
representation of that quantity in a coordinate frame using a 1-dimensional 
array of components in the case of vectors, or a 2-dimensional array in the 
case of tensors. It is unfortunate that the same word is used in English to 
denote a vector and its array representation in a given coordinate frame. 
Regarding tensors, the associated arrays are called matrices. By abuse of 
terminology, we will refer to both tensors and their arrays as matrices, 
although keeping in mind the essential conceptual differences involved. 



2.2 Linear Transformations 

The physical 3-dimensional space is a particular case of a vector space. A 
vector space is a set of objects, called vectors, that follow certain algebraic 
rules. Throughout the book, vectors will be denoted by boldface lower- 
case characters, whereas tensors and their matrix representations will be 
denoted by boldface uppercase characters. Let v, vi, V2, V3, and w be ele- 
ments of a given vector space V, which is defined over the real field, and let 
a and (3 be two elements of this field, i.e., a and (3 are two real numbers. 
Below we summarize the aforementioned rules: 

(*) The sum of vi and V2, denoted by vi + V2, is itself an element of V 
and is commutative, i.e., vi + V2 = V2 + vi; 

(n) V contains an element 0, called the zero vector of V, which, when 
added to any other element v of V, leaves it unchanged, i.e., v+0 = v; 

(*m) The sum defined in (*) is associative, i.e., vi + (v2+V3) = (vi+V2) + 
V3; 

fiv) For every element v of V, there exists a corresponding element, w, 
also of V, which, when added to v, produces the zero vector, i.e., 
V + w = 0. Moreover, w is represented as — v; 

(u) The product av, or va, is also an element of V, for every v of V and 
every real a. This product is associative, i.e., a{f3v) = {af3)v; 

(vi) If a is the real unity, then av is identically v; 

{vii) The product defined in (v) is distributive in the sense that (a) (a + 
j3)v = av + /3v and (6) o:(vi + V2) = avi + o:V2. 

Although vector spaces can be defined over other fields, we will deal with 
vector spaces over the real field unless explicit reference to another field is 
made. Moreover, vector spaces can be either finite- or infinite-dimensional, 
but we will not need the latter. In geometry and elementary mechanics, the 
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dimension of the vector spaces needed is usually three, but when studying 
multibody systems, an arbitrary finite dimension will be required. The 
concept of dimension of a vector space is discussed in more detail later. 

A linear transformation, represented as an operator L, of a vector space 
lA into a vector space V, is a rule that assigns to every vector u of W at 
least one vector v of V, represented as v = Lu, with L endowed with two 
properties: 

(*) homogeneity. L(o:u) = av; and 
(m) additivity. L(ui + U 2 ) = Vi + V 2 . 

Note that, in the foregoing definitions, no mention has been made of 
components, and hence, vectors and their transformations should not be 
confused with their array representations. 

Particular types of linear transformations of the 3-dimensional Euclidean 
space that will be encountered frequently in this context are projections, 
reflections, and rotations. One further type of transformation, which is not 
linear, but nevertheless appears frequently in kinematics, is the one known 
as affine transformation. The foregoing transformations are defined below. 
It is necessary, however, to introduce additional concepts pertaining to 
general linear transformations before expanding into these definitions. 

The range of a linear transformation L of W into V is the set of vectors 
V of V into which some vector u of W is mapped, i.e., the range of L is 
defined as the set of v = Lu, for every vector u of lA. The kernel of L 
is the set of vectors u^r of lA that are mapped by L into the zero vector 
0 G V. It can be readily proven (see Exercises 2. 1-2. 3) that the kernel and 
the range of a linear transformation are both vector subspaces of lA and 
V, respectively, i.e., they are themselves vector spaces, but of a dimension 
smaller than or equal to that of their associated vector spaces. Moreover, 
the kernel of a linear transformation is often called the nullspace of the said 
transformation. 

Henceforth, the 3-dimensional Euclidean space is denoted by . Having 
chosen an origin O for this space, its geometry can be studied in the context 
of general vector spaces. Hence, points of will be identified with vectors 
of the associated 3-dimensional vector space. Moreover, lines and planes 
passing through the origin are subspaces of dimensions 1 and 2, respectively, 
of . Clearly, lines and planes not passing through the origin of are not 
subspaces but can be handled with the algebra of vector spaces, as will be 
shown here. 

An orthogonal projection P of onto itself is a linear transformation of 
the said space onto a plane 77 passing through the origin and having a unit 
normal n, with the properties: 

p2 = P, Pn = 0 (2.1a) 

Any matrix with the first property above is termed idempotent. For n x n 
matrices, it is sometimes necessary to indicate the lowest integer I for which 
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an analogous relation follows, i.e., for which P* = P. In this case, the matrix 
is said to be idempotent of degree 1. 

Clearly, the projection of a position vector p, denoted by p', onto a plane 
n of unit normal n, is p itself minus the component of p along n, i.e., 

p' = p-n(n^p) (2.1b) 

where the superscript T denotes either vector or matrix transposition and 
n^p is equivalent to the usual dot product n • p. 

Now, the identity matrix 1 is defined as the mapping of a vector space 
V into itself leaving every vector v of V unchanged, i.e., 

Iv = V (2.2) 

Thus, p', as given by eq.(2.1b), can be rewritten as 

p' = Ip - nn^p = (1 - nn^)p (2.3) 

and hence, the orthogonal projection P onto U can be represented as 

P = 1 - nn^ (2.4) 

where the product nn^ amounts to a 3 x 3 matrix. 

Now we turn to reflections. Here we have to take into account that re- 
flections occur frequently accompanied by rotations, as yet to be studied. 
Since reflections are simpler to represent, we first discuss these, rotations 
being discussed in full detail in Section 2.3. What we shall discuss in this 
section is pure reflections, i.e., those occurring without any concomitant 
rotation. Thus, all reflections studied in this section are pure reflections, 
but for the sake of brevity, they will be referred to simply as reflections. 

A reflection R of onto a plane U passing through the origin and 
having a unit normal n is a linear transformation of the said space into 
itself such that a position vector p is mapped by R into a vector p' given 

by 

p' = p — 2nn^p = (1 — 2nn^)p 
Thus, the reflection R can be expressed as 

R = 1 - 2nn^ (2.5) 

Prom eq.(2.5) it is then apparent that a pure reflection is represented by a 
linear transformation that is symmetric and whose square equals the iden- 
tity matrix, i.e., R^ = 1. Indeed, s}mimetry is apparent from the equation 
above; the second property is readily proven below: 

R2 = (1 - 2nn^)(l - 2nn^) 

= 1 — 2nn^ — 2nn^ + 4(nn^)(nn^) = 1 — 4nn^ + 4n(n^n)n^ 
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which apparently reduces to 1 because n is a unit vector. Note that from 
the second property above, we find that pure reflections observe a further 
interesting property, namely. 



R 1 = R 

i.e., every pure reflection equals its inverse. This result can be understood 
intuitively by noticing that, upon doubly reflecting an image using two 
mirrors, the original image is recovered. Any square matrix which equals 
its inverse will be termed self-inverse henceforth. 

Further, we take to deriving the orthogonal decomposition of a given 
vector V into two components, one along and one normal to a unit vector 
e. The component of v along e, termed here the axial component V|| — read 
v-par — is simply given as 

V|| = ee^v (2.6a) 

while the corresponding normal component, — read v-perp — is simply 

the difference v — V||, i.e., 

= V — V|| = (1 — ee^)v (2.6b) 

the matrix in parentheses in the foregoing equation being rather frequent 
in kinematics. This matrix will appear when studying rotations. 

Further concepts are now recalled: The basis of a vector space V is a set 
of linearly independent vectors of V, {vj}", in terms of which any vector v 
of V can be expressed as 

V = oiVi + 02 V2 H h a„v„, (2.7) 

where the elements of the set {a*}" are all elements of the field over which 
V is defined, i.e., they are real numbers in the case at hand. The number 
n of elements in the set B = {vj}" is called the dimension of V. Note that 
any set of n linearly independent vectors of V can play the role of a basis of 
this space, but once this basis is defined, the set of real coefficients {a*}" 
for expressing a given vector v is unique. 

Let lA and V be two vector spaces of dimensions m and n, respectively, 
and L a linear transformation of lA into V, and define bases Bu and By for 
lA and V as 

Bu = Bv = {vJ” (2.8) 

Since each Luj is an element of V, it can be represented uniquely in terms 
of the vectors of By, namely, as 

LUj = /yVi + /2jV2 H Vlnj^n, j = (2.9) 



Consequently, in order to represent the images of the m vectors of By, 
namely, the set {Luj}™, nx m real numbers kj, for * = f,...,n and 




24 



2. Mathematical Background 



j = 1, . . . , m, are necessary. These real numbers are now arranged in the 
n X m array [L]gJ^ defined below: 





' ^11 


ll2 


hm 




03 03 

III 


hi 


I 22 


’ ’ hm 


(2.10) 




- hi 


ln2 


hm - 





The foregoing array is thus called the matrix representation of L with 
respect to Bu and By- We thus have an important definition, namely, 

Definition 2.2.1 The jth column of the matrix representation of h with 
respect to the bases Bjj and By is composed of the n real coefficients lij of 
the representation of the image of the jth vector of By in terms of By . 

The notation introduced in eq.(2.10) is rather cumbersome, for it involves 
one subscript and one superscript. Moreover, each of these is subscripted. 
In practice, the bases involved are self-evident, which makes an explicit 
mention of these unnecessary. In particular, when the mapping L is a map- 
ping of lA onto itself, then a single basis suffices to represent L in matrix 
form. In this case, its bracket will bear only a subscript, and no superscript, 
namely, [L]g. Moreover, we will use, henceforth, the concept of basis and 
coordinate frame interchangeably, since one implies the other. 

Two different bases are unavoidable when the two spaces under study 
are physically distinct, which is the case in velocity analyses of manipu- 
lators. As we will see in Chapter 4, in these analyses we distinguish be- 
tween the velocity of the manipulator in Cartesian space and that in the 
joint-rate space. While the Cartesian-space velocity — or Cartesian veloc- 
ity, for brevity — consists, in general, of a 6-dimensional vector containing 
the 3-dimensional angular velocity of the end-effector and the translational 
velocity of one of its points, the latter is an n-dimensional vector. More- 
over, if the manipulator is coupled by revolute joints only, the units of the 
joint-rate vector are all s^^, whereas the Cartesian velocity contains some 
components with units of s^^ and others with units of ms^^. 

Further definitions are now recalled. Given a mapping L of an n-di- 
mensional vector space lA into the n-dimensional vector space V, a nonzero 
vector e that is mapped by L into a multiple of itself, Ae, is called an eigen- 
vector of L, the scalar A being called an eigenvalue of L. The eigenvalues 
of L are determined by the equation 

det(Al-L)=0 (2.11) 

Note that the matrix A1 — L is linear in A, and since the determinant of 
an n X n matrix is a homogeneous nth-order function of its entries, the 
left-hand side of eq.(2.11) is an nth-degree polynomial in A. The foregoing 
pol}momial is termed the characteristic polynomial ofL. Hence, every n x n 
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matrix L has n complex eigenvalues, even if L is defined over the real field. 
If it is, then its complex eigenvalues appear in conjugate pairs. Clearly, 
the eigenvalues of L are the roots of its characteristic polynomial, while 
eq.(2.fl) is called the chMracteristic equation of L. 

Example 2.2.1 What is the representation of the reflection R of into 
itself with respect to the x-y plane, in terms of unit vectors parallel to the 
X, Y, Z axes that form a coordinate frame T ? 

Solution: Note that in this case, lA = V = and, hence, it is not necessary 
to use two different bases for U and V. Now, let i, j, k, be unit vectors 
parallel to the X, Y, and Z axes of a frame T . Clearly, 

Ri = i 

Rj = j 

Rk= -k 



Thus, the representations of the images of i, j and k under R, in T , are 



RiJjr = 


T 

0 


, [Rj]^ = 


'o' 

1 


, [Rk]^ = 


1 

0 O 

1 




0 




0 




-1 



where subscripted brackets are used to indicate the representation frame. 
Hence, the matrix representation of R in T, denoted by [R]y, is 






1 0 0 

0 1 0 

0 0-1 



2.3 Rigid-Body Rotations 

A linear isomorphism, i.e., a one-to-one linear transformation mapping a 
space V onto itself, is called an isometry if it preserves distances between 
any two points of V. If u and v are regarded as the position vectors of two 
such points, then the distance d between these two points is defined as 

d= ^J{n-wY{n-w) ( 2 . 12 ) 

The volume V of the tetrahedron defined by the origin and three points 
of the 3-dimensional Euclidean space of position vectors u, v, and w is 
obtained as one-sixth of the absolute value of the double mixed product of 
these three vectors. 



E = - |u X V • w| 

6 



|det [u 



w 



(2.13) 
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i.e., if a 3x3 array [A] is defined in terms of the components of u, v, and 
w, in a given basis, then the first column of [A] is given by the three 
components of u, the second and third columns being defined analogously. 

Now, let Q be an isometry mapping the triad {u, v, w} into {u', v', w'}. 
Moreover, the distance from the origin to the points of position vectors u, 
V, and w is given simply as ||u||, ||v||, and ||w||, which are defined as 

||u||=V^, ||v|| = V^, \\w\\ = V^^ (2.14) 



Clearly, 



Hull = Hull, Hv1l = Hv||, Hw1l = Hw|| 


(2.15a) 


and 

det [ u' v' w' ] = ±det [ u v w ] 


(2.15b) 


If, in the foregoing relations, the sign of the determinant 
isometry represents a rotation] otherwise, it represents a 
let p be the position vector of any point of f®, its image 
Q being p'. Hence, distance preservation requires that 


is preserved, the 
reflection. Now, 
under a rotation 


T /T / 

P P = P P 


(2.16) 


where 

p' = Qp 

condition (2.16) thus leading to 


(2.17) 


Q^Q = i 


(2.18) 


where 1 was defined in Section 2.2 as the identity 3x3 matrix, and hence, 
eq.(2.18) states that Q is an orthogonal matrix. Moreover, let T and T' 
denote the two matrices defined below: 


T = [u V w], T'=[u' v' w'l 


(2.19) 


from which it is clear that 

T' = QT 


(2.20) 



Now, for a rigid-body rotation, eq.(2.15b) should hold with the positive 
sign, and hence, 

det(T) = det(T') (2.21a) 

and, by virtue of eq.(2.20), we conclude that 

det(Q) = -K (2.21b) 

Therefore, Q is a proper orthogonal matrix, i.e., it is a proper isometry. 
Now we have 

Theorem 2.3.1 The eigenvalues of a proper orthogonal matrix Q lie on 
the unit circle centered at the origin of the complex plane. 
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Proof: Let A be one of the eigenvalues of Q and e the corresponding eigen- 
vector, so that 

Qe = Ae (2.22) 

In general, Q is not expected to be symmetric, and hence, A is not neces- 
sarily real. Thus, A is considered complex, in general. In this light, when 
transposing both sides of the foregoing equation, we will need to take the 
complex conjugates as well. Henceforth, the complex conjugate of a vector 
or a matrix will be indicated with an asterisk as a superscript. As well, the 
conjugate of a complex variable will be indicated with a bar over the said 
variable. Thus, the transpose conjugate of the latter equation takes on the 
form 

e*Q* = Ae* (2.23) 

Multiplying the corresponding sides of the two previous equations yields 

e*Q*Qe = AAe*e (2.24) 

However, Q has been assumed real, and hence, Q* reduces to Q^, the 
foregoing equation thus reducing to 

e*Q^Qe = AAe*e (2.25) 

But Q is orthogonal by assumption, and hence, it obeys eq.(2.18), which 
means that eq.(2.25) reduces to 

e*e=|Ape*e (2.26) 

where | • | denotes the modulus of the complex variable within it. Thus, the 
foregoing equation leads to 

|Ap = 1 (2.27) 

thereby completing the intended proof. As a direct consequence of Theo- 

rem 2.3.1, we have 

Corollary 2 . 3.1 A proper orthogonal 3x3 matrix has at least one eigen- 
value that is -\- 1 . 

Now, let e be the eigenvector of Q associated with the eigenvalue +1. 
Thus, 

Qe = e (2.28) 

What eq.(2.28) states is summarized as a theorem below: 

Theorem 2 . 3.2 (Euler, 1776 ) A rigid-body motion about a point O leaves 
fixed a set of points lying on a line C that passes through O and is parallel 
to the eigenvector e of Q associated with the eigenvalue +1. 

A further result, that finds many applications in robotics and, in general, 
in system theory, is given below: 
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Theorem 2.3.3 (Cayley-Hamilton) LetP{X) he the characteristic poly- 
nomial of an n X n matrix A, i.e., 

P(A) = det(Al - A) = A" + a„_iA"-^ + • • • + aiA + uq (2.29) 

Then A satisfies its characteristic equation, i.e., 

A" + a„_iA"-i + --- + aiA + aol = 0 (2.30) 

where O is the n x n zero matrix. 

Proof: See (Kaye and Wilson, 1998). 



What the Cayley-Hamilton Theorem states is that any power p > n of 
the n X n matrix A can be expressed as a linear combination of the first n 
powers of A — the 0th power of A is, of course, the n x n identity matrix 
1. An important consequence of this result is that any analytic matrix 
function of A can be expressed not as an infinite series, but as a sum, 
namely, a linear combination of the first n powers of A: 1, A, . . . , A"^^ . An 
analytic function f{x) of a real variable x is, in turn, a function with a series 
expansion. Moreover, an analytic matrix function of a matrix argument A 
is defined likewise, an example of which is the exponential function. Prom 
the previous discussion, then, the exponential of A can be written as a 
linear combination of the first n powers of A. It will be shown later that 
any proper orthogonal matrix Q can be represented as the exponential of a 
skew-symmetric matrix derived from the unit vector e of Q, of eigenvalue 
+1, and the associated angle of rotation, as yet to be defined. 



2.3.1 The Cross-Product Matrix 

Prior to introducing the matrix representation of a rotation, we will need a 
few definitions. We will start by defining the partial derivative of a vector 
with respect to another vector. This is a matrix, as described below: In 
general, let u and v be vectors of spaces lA and V, of dimensions m and 
n, respectively. Furthermore, let t be a real variable and / be real- valued 
function of t, u = u(t) and v = v(u(t)) being m- and n-dimensional vector 
functions of t as well, with / = /(u, v). The derivative of u with respect 
to t, denoted by ii(t), is an m-dimensional vector whose *th component is 
the derivative of the *th component of u in a given basis, Ui, with respect 
to t. A similar definition follows for v(t). The partial derivative of / with 
respect to u is an m-dimensional vector whose *th component is the partial 
derivative of / with respect to Ui, with a corresponding definition for the 
partial derivative of / with respect to v. The foregoing derivatives, as all 
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other vectors, will be assumed, henceforth, to be column arrays. Thus, 





■ dffdui - 




- df/dv-i - 




dffdu2 




dffdv2 


du 


-dfidum- 


dv 


-dfldvn- 



Furthermore, the partial derivative of v with respect to u is an n x m 
array whose (*, j) entry is defined as dvijduj, i.e.. 



■ dv\j du\ 


dv\jdu2 


•• dvijdum 


8x2! du\ 


8x2! du2 ■ 


•• 8v2l8um 


.dvnjdui 


dVnj8u2 • 


' ' 8 vji f duyyi 



(2.32) 



Hence, the total derivative of / with respect to u can be written as 



du du V y dv 



(2.33) 



If, moreover, / is an explicit function of t, i.e., if / = /(u, v, t) and 
V = v(u, t), then, one can write the total derivative of / with respect to t 
as 



dt dt V y dt \dv ) dt \dv ) du dt 



(2.34) 



The total derivative of v with respect to t can be written, likewise, as 

dv dv dv du 

dt dt ^ du dt 



(2.35) 



Example 2.3.1 Let the components of v and 'x. in a certain reference 
frame T he given as 



Then 



Hence, 



v]y = 


1 

C<l 

1 


, [x]y = 


to 

1 











[v X x]^ 



V2X3 - V-iX 2 
V3X1 - V1X3 
V1X2 - V2X1 



d(v X x) 




0 


-V3 


V 2 


dx 


T 


V3 

_-V 2 


0 

Vl 


-Vl 

0 



(2.36a) 



(2.36b) 



(2.36c) 
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Henceforth, the partial derivative of the cross product of any 3-dimen- 
sional vectors v and x will be denoted by the 3x3 matrix V. For obvious 
reasons, V is termed the cross-product matrix of vector v. Sometimes the 
cross-product matrix of a vector v is represented as v, but we do not follow 
this notation for the sake of consistency, since we decided at the outset 
to represent matrices with boldface uppercase letters. Thus, the foregoing 
cross product admits the alternative representations 

V X X = Vx (2.37) 

Now, the following is apparent: 



Theorem 2.3.4 The cross-product matrix A of any 3- dimensional vector 
a is skew- symmetric, i.e., 

= -A 



and, as a consequence. 



a X (a X b) = A^b 
where A^ can be readily proven to be 

A? = — ||a|pl + aa^ 

with II • II denoting the Euclidean norm of the vector inside it. 



(2.38) 

(2.39) 



Note that given any 3-dimensional vector a, its cross-product matrix A 
is uniquely defined. Moreover, this matrix is skew-symmetric. The converse 
also holds, i.e., given any 3x3 skew-symmetric matrix A, its associated 
vector is uniquely defined as well. This result is made apparent from Ex- 
ample 2.3.1 and will be discussed further when we define the axial vector 
of an arbitrary 3x3 matrix below. 



2.3.2 The Rotation Matrix 

In deriving the matrix representation of a rotation, we should recall The- 
orem 2.3.2, which suggests that an explicit representation of Q in terms 
of its eigenvector e is possible. Moreover, this representation must contain 
information on the amount of the rotation under study, which is nothing 
but the angle of rotation. Furthermore, line £, mentioned in Euler’s The- 
orem, is termed the axis of rotation of the motion of interest. In order to 
derive the aforementioned representation, consider the rotation depicted in 
Fig. 2.1 of angle f about line C. 

From Fig. 2.1(a), clearly, one can write 

p' =Og + ^ (2.40) 

where OQ is the axial component of p along vector e, which is derived as 
in eq.(2.6a), namely. 



OQ= ee^p 



(2.41) 
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A 





FIGURE 2.1. Rotation of a rigid body about a line. 



Furthermore, from Fig. 2.1b, 

QP'= {cos(p) QP +(sin</>) QP" (2.42) 



with QP being nothing but the normal component of p with respect to e, 
as introduced in eq.(2.6b), i.e., 

QP= (1 — ee^)p (2.43) 

s- 

and QP” given as 

QP"=exp = Ep (2.44) 

Substitution of eqs.(2.44) and (2.43) into eq.(2.42) leads to 
> 

QP'= cos())(l — ee^)p + sin<))Ep (2-45) 

If now eqs.(2.41) and (2.45) are substituted into eq.(2.40), one obtains 
p' = ee^p + cos (}){1 — ee^)p + sin<))Ep (2.46) 

Thus, eq.(2.40) reduces to 

p' = [ee^ + cos <()(1 — ee^) + sin <()E]p (2.47) 

Prom eq.(2.47) it is apparent that p' is a linear transformation of p, the 
said transformation being given by the matrix inside the brackets, which 
is the rotation matrix Q sought, i.e., 

Q = ee^ + cos 4>{1 — ee^) + sin <))E 



(2.48) 
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A special case arises when <f> = n, 

Q = — l + 2ee^, for </> = tt (2.49) 

whence it is apparent that Q is symmetric if <)) = tt. Of course, Q becomes 
symmetric also when <f> = 0, but this is a rather obvious case, leading to 
Q = 1. Except for these two cases, the rotation matrix is not symmetric. 
However, under no circumstance does the rotation matrix become skew- 
symmetric, for a 3 X 3 skew-symmetric matrix is by necessity singular, which 
contradicts the property of proper orthogonal matrices of eq. (2.21b). 

Now one more representation of Q in terms of e and <f> is given. For a 
fixed axis of rotation, i.e., for a fixed value of e, the rotation matrix Q is 
a function of the angle of rotation <f>, only. Thus, the series expansion of Q 
in terms of <f> is 

Q{4>) = Q(0) + Q'm + ^Q"(0)<)>2 + . . . + 1q('^)(0)/ + • • • (2.50) 

where the superscript (k) stands for the A:th derivative of Q with respect to 
4>. Now, from the definition of E, one can readily prove the relations below: 

= (-f)'^E, E^^^ = (-f)'^(l - ee^) (2.51) 

Furthermore, using eqs.(2.48) and (2.51), one can readily show that 

Q(fc)(0) = E'^ (2.52) 



with E defined already as the cross-product matrix of e. Moreover, from 
eqs.(2.50) and (2.52), Q{4>) can be expressed as 

Q{4>) = 1 + E<)> + ^EV" + • • • + + • • • 

whose right-hand side is nothing but the exponential of E<)), i.e., 

Q{4>) = (2.53) 



Equation (2.53) is the exponential representation of the rotation matrix 
in terms of its natural invariants, e and (}). The foregoing parameters are 
termed invariants because they are clearly independent of the coordinate 
axes chosen to represent the rotation under study. The adjective natural is 
necessary to distinguish them from other invariants that will be introduced 
presently. This adjective seems suitable because the said invariants stem 
naturally from Euler’s Theorem. 

Now, in view of eqs.(2.51), the above series can be written as 



ci{4>) 



1 .2 1 .4 1 

+4!'^ + 



(-i)V'= + 



<p - + • • • + 7 — + ■■■ 

^ 3!^ (2A: + 1)!^ ’ ^ 



(1-ee^) 

E 




2.3 Rigid-Body Rotations 33 



The series inside the first brackets is apparently cos</> — 1, while that in 
the second is sin</). We have, therefore, an alternative representation of Q, 
namely, 

Q = 1 + sin (j)Ei + (1 — cos (2-54) 

which is an expected result in view of the Cayley-Hamilton Theorem. 



The Canonical Forms of the Rotation Matrix 

The rotation matrix takes on an especially simple form if the axis of rotation 
coincides with one of the coordinate axes. For example, if the X axis is 
parallel to the axis of rotation, i.e., parallel to vector e, in a frame that we 
will label T, then, we will have 

'll To 0 0 1 To 0 0 ■ 

[6]^-= 0 , [E]a'= 0 0-1, [Y?]x= 0-10 

oj [o 1 0 J [o 0 -1 

In the T-frame, then, 

10 o' 

0 cos(f) — sin</) (2.55a) 

0 sin (f> cos (f> 

Likewise, if we define the coordinate frames y and Z so that their Y and 
Z axes, respectively, coincide with the axis of rotation, then 

cos (f> 0 sin (f> 

[Q]y= 0 1 0 (2.55b) 

— sin (f> 0 cos (f> 




(2.55c) 



The representations of eqs.(2.55a-c) can be called the X-, Y-, and Z- 
canonical forms of the rotation matrix. In many instances, a rotation matrix 
cannot be derived directly from information on the original and the final 
orientations of a rigid body, but the overall motion can be readily decom- 
posed into a sequence of simple rotations taking the above canonical forms. 
An application of canonical forms lies in the parameterization of rotations 
by means of Euler angles, consisting of three successive rotations, 0 
and 'ip, about one axis of a coordinate frame. Euler angles are introduced 
in Exercise 2.18, and applications thereof in Exercises 2.35, 2.36, 3.9, and 
3.10. 
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2.3.3 The Linear Invariants of a 3 x 3 Matrix 

Now we introduce two linear invariants of 3 x 3 matrices. Given any 3x3 
matrix A, its Cartesian decomposition, the counterpart of the Cartesian 
representation of complex numbers, consists of the sum of its symmetric 
part, As, and its skew-symmetric part. Ass, defined as 

As = i(A + A^), Ass=\{A-A^) (2.56) 

The axial vector or for brevity, the vector of A, is the vector a with the 
property 

a X V = Assv (2-57) 

for any 3-dimensional vector v. The trace of A is the sum of the eigenvalues 
of As, which are real. Since no coordinate frame is involved in the above 
definitions, these are invariant. When calculating these invariants, of course, 
a particular coordinate frame must be used. Let us assume that the entries 
of matrix A in a certain coordinate frame are given by the array of real 
numbers Oij, for i,j = 1, 2, 3. Moreover, let a have components a*, for i = 
1, 2, 3, in the same frame. The above-defined invariants are thus calculated 
as 



vect(A) = a = - 



a32 — 0 - 21 , 

Oil — Oil 
021 — Ol2 



tr(A) = ail + 022 + Oil (2.58) 



Prom the foregoing definitions, the following is now apparent: 

Theorem 2.3.5 The vector of a 2> x 2> matrix vanishes if and only if it is 
symmetric, whereas the trace of an n x n matrix vanishes if the matrix is 
.skew symmetric. 

Other useful relations are given below. For any 3-dimensional vectors a 
and b, 

vect(ab^) = “ 2 ^ ^ ^ (2.59) 

and 

tr(ab^) = a^b (2.60) 

The second relation is quite straightforward, but the first one is less so; a 
proof of the first relation is given below: Let w denote vect(ab^). Prom 
Definition (2.57), for any 3-dimensional vector v. 



w X V = Wv (2-61) 

where W is the skew-symmetric component of ab^, namely, 

W = i(ab^ -ba^) 



(2.62) 
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and hence, 

Wv = w X V = i[(b^v)a — (a^v)b] (2.63) 

Now, let us compare the last expression with the double cross product 
(b X a) X V, namely. 



(b X a) X V = (b^v)a — (a^v)b (2.64) 

from which it becomes apparent that 

w = -b X a (2.65) 

and the aforementioned relation readily follows. 

Note that Theorem 2.3.5 states a necessary and sufficient condition for 
the vanishing of the vector of a 3 x 3 matrix, but only a sufficient condition 
for the vanishing of the trace of an n x n matrix. What this implies is that 
the trace of an n x n matrix can vanish without the matrix being necessar- 
ily skew symmetric, but the trace of a skew-symmetric matrix necessarily 
vanishes. Also note that whereas the vector of a matrix is defined only for 
3x3 matrices, the trace can be defined more generally for n x n matrices. 

2.3.4 The Linear Invariants of a Rotation 

From the invariant representations of the rotation matrix, eqs.(2.48) and 
(2.54), it is clear that the first two terms of Q, ee^ and cos<))(l — ee^), are 
symmetric, whereas the third one, sin<))E, is skew-symmetric. Hence, 

vect(Q) = vect(sin ()) E) = sin ())e (2.66) 



whereas 



tr(Q) = tr[ee^ + cos (f>{l — ee^)] = e^e + cos (p{3 — e^e) 

from which one can readily solve for cos (f>, namely, 

, tr(Q) - 1 
cos 4> = 



1 + 2 cos 4> 

(2.67) 

( 2 . 68 ) 



Henceforth, the vector of Q will be denoted by q and its components in a 
given coordinate frame by i/i, 1 / 2 , and 1 / 3 . Moreover, rather than using tr(Q) 
as the other linear invariant, = cos(f> will be introduced to refer to the 
linear invariants of the rotation matrix. Hence, the rotation matrix is fully 
defined hy four scalar parameters, namely which will be conveniently 

stored in the 4-dimensional array A, defined as 



^ = [qi, <? 2 , Q3, qo]'^ 



(2.69) 
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Note, however, that the four components of A are not independent, for 
they obey the relation 

||q|p + = sin^ (/) + cos^ (/) = 1 (2.70) 

Thus, eq.(2.70) can be written in a more compact form as 

= 91 + 92 + 93 + = 1 (2-71) 

What eq.(2.70) states has a straightforward geometric interpretation: As 
a body rotates about a fixed point, its motion can be described in a 4- 
dimensional space by the motion of a point of position vector A that moves 
on the surface of the unit sphere centered at the origin of the said space. 
Alternatively, one can conclude that, as a rigid body rotates about a fixed 
point, its motion can be described in a 3-dimensional space by the motion 
of position vector q, which moves within the unit solid sphere centered at 
the origin of the said space. Given the dependence of the four components 
of vector A, one might be tempted to solve for, say, from eq.(2.70) in 
terms of the remaining components, namely, as 



9o — iyl — (9i + 92 + 93 ) (2.72) 



This, however, is not a good idea because the sign ambiguity of eq.(2.72) 
leaves angle <f> undefined, for qo is nothing but cos(f>. Moreover, the three 
components of vector q alone, i.e., sin<))e, do not suffice to define the ro- 
tation represented by Q. Indeed, from the definition of q, one has 

sin<)) = ±||q||, e = q/sin<)) (2.73) 



from which it is clear that q alone does not suffice to define the rotation 
under study, since it leaves angle <f> undefined. Indeed, the vector of the 
rotation matrix provides no information about cos(f>. Yet another repre- 
sentation of the rotation matrix is displayed below, in terms of its linear 
invariants, that is readily derived from representations (2.48) and (2.54), 
namely. 



Q 



qq 



90 



qq 

l|q|| 



+ Q 



(2.74a) 



in which Q is the cross-product matrix of vector q, i.e.. 



Q 



d(q X x) 
dx 



for any vector x. 

Note that by virtue of eq.(2.70), the representation of Q given in eq.(2.74a) 
can be expressed alternatively as 



Q 



9o 1 + Q + 



qq 



T 



1 + 9o 



(2.74b) 
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From either eq.(2.74a) or eq. (2.74b) it is apparent that linear invariants 
are not suitable to represent a rotation when the associated angle is either 
7T or close to it. Note that a rotation through an angle <f> about an axis 
given by vector e is identical to a rotation through an angle —(f> about an 
axis given by vector — e. Hence, changing the sign of e does not change the 
rotation matrix, provided that the sign of <f> is also changed. Henceforth, 
we will choose the sign of the components of e so that sin <f> > 0, which is 
equivalent to assuming that 0 < <f> < n. Thus, sin<)) is calculated as ||q||, 
while cos(f> as indicated in eq.(2.68). Obviously, e is simply q normalized, 
i.e., q divided by its Euclidean norm. 



2.3.5 Examples 

The examples below are meant to stress the foregoing ideas on rotation 
invariants. 



Example 2.3.2 If [e]^ = [V3/3, -a/3/3, a/3/3 in a given coordinate 
frame T and <p = 120°, what is Q in IF ? 

Solution: From the data. 



1 ^ 

cos (f> = — , sm (f> = 



Moreover, in the T frame, 

r T 1 1 

[ee \t = 
and hence, 

[ 1 - ee^ \t = : 



■ 1 ■ 




1 


1 


-1 


1 ■ 




-1 


[1 


I 1 ] = T 


-1 


1 


-1 




1 




6 


1 


-1 


1 




■ 2 


1 - 1 ' 




a/3 


'0 


-1 


- 1 ' 


1 


2 1 


, [ E^e 


_ V 
“ q 


1 


0 


-1 


-1 


1 2 




o 


1 


1 


0 



Thus, from eq.(2.48). 



■ 1 


-1 


1 ■ 


1 


■ 2 


1 


- 1 ' 


Q 


'0 


-1 


- 1 ' 


-1 


1 


-1 


1 


2 


1 


o 

- j - — 


1 


0 


-1 


1 


-1 


1 


6 


-1 


1 


2 


6 


1 


1 


0 



i.e.. 



[Q].f 



0-10 
0 0-1 
1 0 0 
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Example 2.3.3 The matrix representation of a linear transformation Q 
in a certain reference frame T is given below. Find out whether the said 
transformation is a rigid-body rotation. If it is, find its natural invariants. 



[Q].F 



0 1 0 
0 0 1 
1 0 0 



Solution: First the given array is tested for orthogonality: 






'0 


1 


o' 




'0 


0 


1' 




■f 


0 


o' 


0 


0 


1 




1 


0 


0 


= 


0 


1 


0 


1 


0 


1 

o 




0 


1 


0 




0 


0 


1 



thereby showing that the said array is indeed orthogonal. Thus, the linear 
transformation could represent a reflection or a rotation. In order to de- 
cide which one this represents, the determinant of the foregoing array is 
computed: 

det( Q ) = Tl 

which makes apparent that Q indeed represents a rigid-body rotation. Now, 
its natural invariants are computed. The unit vector e can be computed 
as the eigenvector of Q associated with the eigenvalue +1. This requires, 
however, finding a nontrivial solution of a homogeneous linear system of 
three equations in three unknowns. This is not difficult to do, but it is 
cumbersome and is not necessary. In order to And e and it is recalled 
that vect(Q) = sin</)e, which is readily computed with differences only, as 
indicated in eq.(2.58), namely. 



[q]^ = sin</)[e]^ 



1 

2 



1 

1 

1 



Under the assumption that sin </> > 0, then, 

sm4>= ||q|| = ^ 

and hence, 

1 ' 

1 
1 

and 

4 > = 60° or 120° 

The foregoing ambiguity is resolved by the trace of Q, which yields 
1 + 2cos(/) = tr(Q) = 0, cos </)=-- 



= 



[q] 



T 



3 
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The negative sign of cos 4> indicates that 4> lies in the second quadrant — it 
cannot he in the third quadrant because of our assumption about the sign 
of sin (f > — and hence 

4 > = f20° 

Example 2.3.4 A coordinate frame Xi, Yi, Zi is rotated into a configu- 
ration X 2 , Y 2 , Z 2 in such a way that 

X2 = -Yi, Y2 = Yi, Y2 = -Xi 

Find the matrix representation of the rotation in X\, Yi, Z\ coordinates. 
From this representation, compute the direction of the axis and the angle 
of rotation. 

Solution: Let ii, ji, ki be unit vectors parallel to X\, Y\, Z\, respectively, 
I 2 , j 2 , k 2 being defined correspondingly. One has 



12 = -ji, j2=ki, k2 = -ii 



and hence, from Definition 2.2.1, the matrix representation [Q]i of the 
rotation under study in the X\, Y\, Z\ coordinate frame is readily derived: 



[Q]i 



00-1 
-100 
0 1 0 



from which the linear invariants follow, namely. 



[q]i = [vect(Q)]i = sin</)[e]i 



1 

2 



1 

-1 

-1 



cos 4> 



^[tr(Q)-l] 



1 

2 



Under our assumption that sin</> > 0, we obtain 

1 ■ 

-1 
-1 

From the foregoing values for sin f and cos angle f is computed uniquely 
as 

4> = 120° 

Example 2.3.5 Show that the matrix P given in eq.(2.f) satisfies proper- 
ties (2.1a). 

Solution: First, we prove idempotency, i.e., 

= (1 — mi^)(l — mi^) 

= 1 — 2mi^ + mi^mi^ = 1 — mi^ = P 



sin (f> -- 



2 



V ^ r 1 

[e]i 



[q]i 

sin (f> 



3 
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thereby showing that P is, indeed, idempotent. Now we prove that n is an 
eigenvector of P with eigenvalue, 0 and hence, n spans the nullspace of P. 
In fact, 

Pn = (1 — nn^)n = n — nn^n = n — n = 0 

thereby completing the proof. 

Example 2.3.6 The representations of three linear transformations in a 
given coordinate frame T are given below: 

2 1 2 ' 

-2 2 1 
-1 -2 2 

2 1 1 ■ 

1 2 -1 
1 -1 2 

12 2 ■ 

2 1 -2 
2 -2 1 

One of the foregoing matrices is an orthogonal projection, one is a reflec- 
tion, and one is a rotation. Identify each of these and give its invariants. 






Solution: From representations (2.48) and (2.54), it is clear that a rotation 
matrix is symmetric if and only if sin<)) = 0. This means that a rotation 
matrix cannot be symmetric unless its angle of rotation is either 0 or tt, 
i.e., unless its trace is either 3 or —1. Since [B]jc and [C]jc are symmetric, 
they cannot be rotations, unless their traces take on the foregoing values. 
Their traces are thus evaluated below: 



tr(B) = 2, tr(C) = 1 



which thus rules out the foregoing matrices as suitable candidates for ro- 
tations. Thus, A is the only candidate left for proper orthogonality, its 
suitability being tested below: 



[ AA^ ]jr 



1 

9 



9 0 0 
0 9 0 
0 0 9 



det(A) = +1 



and hence, A indeed represents a rotation. Its natural invariants are next 
computed: 



sin<))[e]jc- = [vect(A)]jc 



1 

2 



-1 

1 

-1 



cos<)>= i[tr(A)-l] = ^(2-1) 



1 

2 
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We assume, as usual, that sin </> > 0. Then, 

\/3 

sin</) = ||vect(A)|| = i.e., <j) = 60° 

Moreover, 

[vect(A)]^ a/3 

Now, one matrix of B and C is an orthogonal projection and the other is 
a reflection. To be a reflection, a matrix has to be orthogonal. Hence, each 
matrix is tested for orthogonality: 






6 3 3 

3 6-3 

3-3 6 



[CC^b=i 



9 0 0 
0 9 0 
0 0 9 



thereby showing that C is orthogonal and B is not. Furthermore, det(C) = 
— 1, which confirms that C is a reflection. Now, if B is a projection, it is 
bound to be singular and idempotent. Prom the orthogonality test it is clear 
that it is idempotent. Moreover, one can readily verify that det(B) = 0, 
and hence B is singular. The unit vector [n]jr = [ni, n- 2 , spanning 

its nullspace is determined from the general form of projections, eq.(2.1a), 
whence it is apparent that 



nn^ = 1 — B 

Therefore, if a solution n has been found, then — n is also a solution, i.e., the 
problem admits two solutions, one being the negative of the other. These two 
solutions are found below, by first rewriting the above system of equations 
in component form: 



'n-i 


1 


1 -1 - 1 ' 


nin,2 n-2 


o 


-1 1 1 


«-2«-3 «-3 


6 


-1 1 1 



Now, from the diagonal entries of the above matrices, it is apparent that the 
three components of n have identical absolute values, i.e., a/3/3. Moreover, 
from the off-diagonal entries of the same matrices, the second and third 
components of n bear equal signs, but we cannot tell whether positive or 
negative, because of the quadratic nature of the problem at hand. The two 
solutions are thus obtained as 



n 




1 

-1 

-1 



which is the only invariant of B. 
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We now look at C, which is a reflection, and hence, bears the form 

C = 1 - 2nn^ 

In order to determine n, note that 

nn^ = -(l-C) 

or in component form. 



n\ nin -2 


1 


1 -1 -1' 


nin,2 n-2 


o 


-1 1 1 


«-2«-3 «-3 


6 


-1 1 1 



which is identical to the matrix equation derived in the case of matrix B. 
Hence, the solution is the same, i.e., 

1 ■ 

-1 
-1 

thereby finding the invariant sought. 




Example 2.3.7 The vector and the trace of a rotation matrix Q, in a 
certain reference frame T , are given as 



[vect(Q)]^ 



1 

2 



-1 

1 

-1 



tr(Q) = 2 



Find the matrix representation of Q in the given coordinate frame and in 
a frame having its Z-axis parallel to vect(Q), 



Solution: We shall resort to eq. (2.74a) to determine the rotation matrix 
Q. The quantities involved in the aforementioned representation of Q are 
readily computed, as shown below: 



[qq^].F = \ 



1 -1 1 

-1 1 -1 

1 -1 1 



l|q|| 



2 



3 

4’ 




0 1 1 

-1 0 1 

-1 -1 0 



from which Q follows: 



[Q].f 



1 

3 



2 1 2 

-2 2 1 

-1 -2 2 



in the given coordinate frame. Now, let Z denote a coordinate frame whose 
Z-axis is parallel to q. Hence, 



'o' 


, [W']z = \ 


'0 


0 


o' 




'0 


-1 


o' 


0 


0 


0 


0 


, [Q]z = — 


1 


0 


0 


1 


4 


0 


0 


1 


L 


0 


0 


0 
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which readily leads to 

1/2 -a/3/2 o' 

[Q]z= V3/2 1/2 0 

0 0 1 

and is in the Z-canonical form. 

Example 2.3.8 A procedure for trajectory planning produced a matrix rep- 
resenting a rotation for a certain pick-and-place operation, as shown below: 

'0.433 -0.500 z 
[Q]= X 0.866 -0.433 

[0.866 y 0.500 

where x, y, and z are entries that are unrecognizable due to failures in the 
printing hardware. Knowing that Q is in fact a rotation matrix, find the 
missing entries. 

Solution: Since Q is a rotation matrix, the product P = Q^Q should equal 
the 3x3 identity matrix, and det(Q) should be +1. The foregoing product 
is computed first: 

'0.437 + ^2 0.433(x-z-l) 0.5(-y + z) + 0.375 ' 

[P]^= * 0.937 + x2 0.866(x + y) -0.216 

* * 1 + 

where the entries below the diagonal have not been printed because the 
matrix is symmetric. Upon equating the diagonal entries of the foregoing 
array to unity, we obtain 

X = ±0.250, y = 0, z = ±0.750 

while the vanishing of the off-diagonal entries leads to 

X = 0.250, y = 0, z = -0.750 

which can be readily verified to produce det(Q) = ±1. 



2.3.6 The Euler- Rodrigues Parameters 

The invariants defined so far, namely, the natural and the linear invariants 
of a rotation matrix, are not the only ones that are used in kinematics. 
Additionally, one has the Euler parameters, or Euler-Rodrigues parameters, 
as Cheng and Gupta (1989) propose that they should be called, represented 
here as r and rg. The Euler-Rodrigues parameters are defined as 




(2.75) 
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One can readily show that Q takes on a quite simple form in terms of 
the Euler-Rodrigues parameters, namely, 

Q = (ro^ — r • r)l + 2rr^ + 2roR (2.76) 

in which R is the cross-product matrix of r, i.e., 

d(r X x) 



for arbitrary x. 

Note that the Euler-Rodrigues parameters appear quadratically in the 
rotation matrix. Hence, these parameters cannot be computed with sim- 
ple sums and differences. A closer inspection of eq.(2.74b) reveals that the 
linear invariants appear almost linearly in the rotation matrix. This means 
that the rotation matrix, as given by eq. (2.74b), is composed of two types 
of terms, namely, linear and rational. Moreover, the rational term is com- 
posed of a quadratic expression in the numerator and a linear expression 
in the denominator, the ratio thus being linear, which explains why the 
linear invariants can be obtained by sums and differences from the rotation 
matrix. 

The relationship between the linear invariants and the Euler-Rodrigues 
parameters can be readily derived, namely. 



ro 



± 



1 + go 

2 



r 




4)^71 



(2.77) 



Furthermore, note that, if <)) = tt, then tq = 0, and formulae (2.77) fail 
to produce r. However, from eq.(2.75). 



For (f) = tt: r = e, tq = 0 



(2.78) 



We now derive invariant relations between the rotation matrix and the 
Euler-Rodrigues parameters. To do this, we resort to the concept of ma- 
trix square root. As a matter of fact, the square root of a square matrix is 
nothing but a particular case of an analytic function of a square matrix, 
discussed in connection with Theorem 2.3.3 and the exponential represen- 
tation of the rotation matrix. Indeed, the square root of a square matrix is 
an analytic function of that matrix, and hence, admits a series expansion in 
powers of the matrix. Moreover, by virtue of the Cayley-Hamilton Theorem 
(Theorem 2.3.3) the said square root should be, for a 3 x 3 matrix, a linear 
combination of the identity matrix 1 , the matrix itself, and its square, the 
coefficients being found using the eigenvalues of the matrix. 

Furthermore, from the geometric meaning of a rotation through the angle 
4> about an axis parallel to the unit vector e, it is apparent that the square 
of the matrix representing the foregoing rotation is itself a rotation about 
the same axis, but through the angle 2<f>. By the same token, the square 
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root of the same matrix is again a rotation matrix about the same axis, 
but through an angle Now, while the square of a matrix is unique, its 
square root is not. This fact is apparent for diagonalizable matrices, whose 
diagonal entries are their eigenvalues. Each eigenvalue, whether positive 
or negative, admits two square roots, and hence, a diagonalizable n x n 
matrix admits as many square roots as there are combinations of the two 
possible roots of individual eigenvalues, disregarding rearrangements of the 
latter. Such a number is 2", and hence, a 3 x 3 matrix admits eight square 
roots. For example, the eight square roots of the identity 3x3 matrix are 
displayed below: 



'1 


0 


o ' 




'1 


0 


0 ■ 




'1 


0 


o ' 




'-1 


0 


o ' 


0 


1 


0 


7 


0 


1 


0 


7 


0 


-1 


0 


7 


0 


1 


0 


1 

O 


0 


1 




0 


0 


-1 




0 


0 


1 




0 


0 


1 



'1 


0 


0 ' 




'-1 


0 


0 ' 




'-1 


0 


o ' 




'-1 


0 


0 ' 


0 


-1 


0 


7 


0 


1 


0 


7 


0 


-1 


0 


7 


0 


-1 


0 


1 

o 


0 


-1 




0 


0 


-1 




0 


0 


1 




0 


0 


-1 



In fact, the foregoing result can be extended to orthogonal matrices as 
well and, for that matter, to any square matrix with n linearly indepen- 
dent eigenvectors. That is, an n x n orthogonal matrix admits 2" square 
roots. However, not all eight square roots of a 3 x 3 orthogonal matrix are 
orthogonal. In fact, not all eight square roots of a 3 x 3 proper orthogonal 
matrix are proper orthogonal either. Of these square roots, nevertheless, 
there is one that is proper orthogonal, the one representing a rotation of 
4>/2. We will denote this particular square root of Q by a/Q. The Euler- 
Rodrigues parameters of Q can thus be expressed as the linear invariants 
of VQ, namely. 



r = vect(yq), ro = (2.79) 

It is important to recognize the basic differences between the linear in- 
variants and the Euler-Rodrigues parameters. Whereas the former can be 
readily derived from the matrix representation of the rotation involved by 
simple additions and subtractions, the latter require square roots and en- 
tail sign ambiguities. However, the former fail to produce information on 
the axis of rotation whenever the angle of rotation is tt, whereas the latter 
produce that information for any value of the angle of rotation. 

The Euler-Rodrigues parameters are nothing but the quaternions in- 
vented by Sir William Rowan Hamilton (1844) in an extraordinary moment 
of creativity on Monday, October 16, 1843, as “Hamilton, accompanied by 
Lady Hamilton, was walking along the Royal Canal in Dublin towards the 
Royal Irish Academy, where Hamilton was to preside a meeting.” (Altmann, 
1989). 
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Moreover, the Euler- Rodrigues parameters should not be confused with 
the Euler angles, which are not invariant and hence, admit multiple defi- 
nitions. The foregoing means that no single set of Euler angles exists for 
a given rotation matrix, the said angles depending on how the rotation is 
decomposed into three simpler rotations. For this reason, Euler angles will 
not be stressed here. The reader is referred to Exercise 18 for a short dis- 
cussion of Euler angles; Synge ( 1960 ) includes a classical treatment, while 
Kane, Likins and Levinson provide an extensive discussion of the same. 

Example 2.3.9 Find the Euler-Rodrigues parameters of the proper orthog- 
onal matrix Q given as 



Q 



1 

3 



-12 2 
2-12 
2 2-1 



Solution: Since the given matrix is symmetric, its angle of rotation is tt 
and its vector linear invariant vanishes, which prevents us from finding 
the direction of the axis of rotation from the linear invariants; moreover, 
expressions ( 2 . 77 ) do not apply. However, we can use eq.( 2 . 49 ) to find the 
unit vector e parallel to the axis of rotation, i.e., 

ee^ = ^(1 + Q) 

or in component form. 



e\ 6162 6163 


1 


■f 1 1' 


6162 eff 6263 


0 


1 1 1 


_6i63 6263 6§ 


0 


1 1 1 



A simple inspection of the components of the two sides of the above equa- 
tion reveals that all three components of e are identical and moreover, of 
the same sign, but we cannot tell which sign this is. Therefore, 



e 




1 

1 

1 



Moreover, from the symmetry of Q, we know that <j) = tt, and hence. 



r 



esin 



2 



± 



V3 



ro = cos 
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2.4 Composition of Reflections and Rotations 

As pointed out in Section 2.2, reflections occur often accompanied by ro- 
tations. The effect of this combination is that the rotation destroys the 
two properties of pure reflections, symmetry and self-inversion, as defined 
in Section 2.2. Indeed, let R be a pure reflection, taking on the form ap- 
pearing in eq.(2.5), and Q an arbitrary rotation, taking on the form of 
eq.(2.48). The product of these two transformations, QR, denoted by T, 
is apparently neither symmetric nor self-inverse, as the reader can readily 
verify. Likewise, the product of these two transformations in the reverse 
order is neither symmetric nor self- inverse. 

As a consequence of the foregoing discussion, an improper orthogonal 
transformation that is not symmetric can always be decomposed into the 
product of a rotation and a pure reflection, the latter being symmetric 
and self-inverse. Moreover, this decomposition can take on the form of 
any of the two possible orderings of the rotation and the reflection. Note, 
however, that once the order has been selected, the decomposition is not 
unique. Indeed, if we want to decompose T in the above paragraph into 
the product QR, then we can freely choose the unit normal n of the plane 
of reflection and write 

R = 1 - 2nn^ 

vector n then being found from 

nn^ = - ( 1 — R) 

2^ ' 

Hence, the factor Q of that decomposition is obtained as 

Q = TR 1 = TR = T - 2(Tn)n^ 

where use has been made of the self-inverse property of R. Any other 
selection of vector n will lead to a different decomposition of T. 

Example 2.4.1 Join the palms of your two hands in the position adopted 
by swimmers when preparing for plunging, while holding a sheet of paper 
between them. The sheet defines a plane in each hand that we will call the 
hand plane, its unit normal, pointing outside of the hand, being called the 
hand normal and represented as vectors Ur and Ur for the right and left 
hand, respectively. Moreover, let Or and Or denote unit vectors pointing 
in the direction of the finger axes of each of the two hands. Thus, in the 
swimmer position described above, xir = —Ur and Or = Or. Now, without 
moving your right hand, let the left hand attain a position whereby the 
left-hand normal lies at right angles with the right-hand normal, the palm 
pointing downwards and the finger axes of the two hands remaining parallel. 
Find the representation of the transformation carrying the right hand to the 
final configuration of the left hand, in terms of the unit vectors Ur and Or. 
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Solution: Let us regard the desired transformation T as the product of a 
rotation Q by a pure reflection R, in the form T = QR. Thus, the trans- 
formation occurs so that the reflection takes place first, then the rotation. 
The reflection is simply that mapping the right hand into the left hand, 
and hence, the reflection plane is simply the hand plane, i.e., 

R = 1 - 2nRH^ 

Moreover, the left hand rotates from the swimmer position about an axis 
parallel to the Anger axes through an angle of 90° clockwise from your 
viewpoint, i.e., in the positive direction of vector or. Hence, the form of 
the rotation involved can be derived readily from eq.(2.48) and the above 
information, namely, 

Q = oro]^ + Or 

where Or is the cross-product matrix of or. Hence, upon performing the 
product QR, we have 

T = orOr + 20 R - 2{or X 

which is the transformation sought. 



2.5 Coordinate Transformations and Homogeneous 
Coordinates 

Crucial to robotics is the unambiguous description of the geometrical re- 
lations among the various bodies in the environment surrounding a robot. 
These relations are established by means of coordinate frames, or frames, 
for brevity, attached to each rigid body in the scene, including the robot 
links. The origins of these frames, moreover, are set at landmark points 
and orientations defined by key geometric entities like lines and planes. For 
example, in Chapter 4 we attach two frames to every moving link of a serial 
robot, with origin at a point on each of the axis of the two joints coupling 
this link with its two neighbors. Moreover, the Z-axis of each frame is de- 
fined, according to the Denavit-Hartenberg notation, introduced in that 
chapter, along each joint axis, while the X-axis of the frame closer to the 
base — termed the fore frame — is defined along the common perpendicular 
to the two joint axes. The origin of the same frame is thus defined as the in- 
tersection of the fore axis with the common perpendicular to the two axes. 
This section is devoted to the study of the coordinate transformations of 
vectors when these are represented in various frames. 
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2.5.1 Coordinate Transformations Between Frames 
vnth a Common Origin 

We will refer to two coordinate frames in this section, namely, A = {X, Y, Z} 
and B = {X, y, Z}. Moreover, let Q be the rotation carrying A into B, 
i.e., 

Q: A ^ B (2.80) 

The purpose of this subsection is to establish the relation between the 
representations of the position vector of a point P in M and B, denoted by 
[p]yl and [p]b, respectively. Let 



[pU 



X 

y 

z 



(2.81) 



We want to find [p]g in terms of [p ]^i and Q, when the latter is represented 
in either frame. The coordinate transformation can best be understood if 
we regard point P as attached to frame A, as if it were a point of a box 
with sides of lengths x, y, and z, as indicated in Fig. 2.2a. Now, frame A 
undergoes a rotation Q about its origin that carries it into a new attitude, 
that of frame B, as illustrated in Fig. 2.2b. Point P in its rotated position 
is labeled 77, of position vector tt, i.e., 

7T = Qp (2.82) 

It is apparent that the relative position of point P with respect to its box 
does not change under the foregoing rotation, and hence. 



Moreover, let 






X 

y 

z 






e 

V 

c 



(2.83) 



(2.84) 



The relation between the two representations of the position vector of any 
point of the 3-dimensional Euclidean space is given by 



Theorem 2.5.1 The representations of the position vector tt of any point 
in two frames A andB, denoted by [t^\a respectively, are related 

by 



[ t^]a = [Q]H[7r]B 



(2.85) 
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(a) 



(b) 



FIGURE 2.2. Coordinate transformation: (a) coordinates of point P in the 
M-frame; and (b) relative orientation of frame B with respect to A. 

Proof: Let us write eq.(2.82) in A'. 

[7t]yi = [Q]yi[p]yi (2.86) 

Now, from Fig. 2.2b and eqs.(2.81) and (2.83) it is apparent that 

[ t ^] b =[ v]a (2.87) 

Upon substituting eq.(2.87) into eq.(2.86), we obtain 

[t^]a = [CI]a[t^]b ( 2 . 88 ) 



q.e.d. Moreover, we have 

Theorem 2.5.2 The representations of Q carrying A into B in these two 
frames are identical, i.e., 

[QU = [Q]b (2.89) 



Proof: Upon substitution of eq.(2.82) into eq.(2.85), we obtain 

[Qp]u = [Q]u[Qp]b 

or 

[Q]u[p]u = [Q]u[Qp]b 

Now, since Q is orthogonal, it is nonsingular, and hence, [Q]^i can be 
deleted from the foregoing equation, thus leading to 

[p]u = [Q]b[p]b (2.90) 

However, by virtue of Theorem 2.5.1, the two representations of p observe 
the relation 



[p]u = [Q]u[p]b 



(2.91) 
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the theorem being proved upon equating the right-hand sides of eqs.(2.90) 
and (2.91). 

Note that the foregoing theorem states a relation valid only for the con- 
ditions stated therein. The reader should not conclude from this result that 
rotation matrices have the same representations in every frame. This point 
is stressed in Example 2.5.1. Furthermore, we have 

Theorem 2.5.3 The inverse relation of Theorem 2.5.1 is given by 

[7t]b= [Q^]b[7tU (2.92) 



Proof: This is straightforward in light of the two foregoing theorems, and 
is left to the reader as an exercise. 

Example 2.5.1 Coordinate frames A and B are shown in Fig. 2.3. Find 
the representations of Q rotating A into B in these two frames and show 
that they are identical. Moreover, if [p]yi = [1, 1, 1]^, find [p]b- 

Solution: Let i, j, and k be unit vectors in the directions of the X-, Y-, 
and Z-axes, respectively; unit vectors t, 7 , and k are defined likewise as 
parallel to the X-, y~, and ^-axes of Fig. 2.3. Therefore, 

Qi = t = -k, Qj=7=-i, Qk = K;=j 

Therefore, using Definition 2.2.1, the matrix representation of Q carrying 
A into B, m A, is given by 




FIGURE 2.3. Coordinate frames A and B with a common origin. 
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Now, in order to find [Q]b, we apply Q to the three unit vectors of B, l, 
7 , and k. Thus, for t, we have 



■ 0 -1 o' 




■ 0 ■ 




■ 0 ■ 


0 0 1 




0 


= 


-1 


-10 0 




-1 




1 

o 
1 



Likewise, 



Q7=-t, Qtc=7 



again, from Definition 2.2.1, we have 



[Q]b 



0-10 
0 0 1 
-10 0 



[QU 



thereby confirming Theorem 2.5.2. Note that the representation of this 
matrix in any other coordinate frame would be different. For example, if 
we represent this matrix in a frame whose X-axis is directed along the axis 
of rotation of Q, then we end up with the X-canonical representation of 
Q, namely. 



[QU 



1 0 0 
0 cos (f> — sin (f> 

0 sin (f> cos (f> 



with the angle of rotation 4> being readily computed as 4> 
thus yields 



[QU 



1 0 0 
0 -1/2 -a/3/2 

0 a/3/2 -1/2 



120°, which 



which apparently has different entries from those of [ Q ]yi and [ Q ]b found 
above. 

Now, from eq.(2.92). 



■ 0 0 - 1 ' 




T 




■- 1 ' 


-10 0 




1 


= 


-1 


0 1 0 




1 




1 



a result that can be readily verified by inspection. 



2.5.2 Coordinate Transformation with Origin Shift 

Now, if the coordinate origins do not coincide, let b be the position vector 
of O, the origin of B, from O, the origin of A, as shown in Fig. 2.4. The 
corresponding coordinate transformation from A to B, the counterpart of 
Theorem 2.5.1, is given below. 
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Theorem 2.5.4 The representations of the position vector p of a point P 
of the Euclidean 3- dimensional space in two frames A and B are related by 

[p]h = [b]yi + [Q]yi[7r]B (2.93a) 

[7t]b = [Q^]B([-b]^ + [p]^) (2.93b) 

with b defined as the vector directed from the origin of A to that of B, and 
7T the vector directed from the origin of B to P, as depicted in Fig. 2.f. 

Proof: We have, from Fig. 2.4, 

p = b + 7T (2.94) 

If we express the above equation in the ^-frame, we obtain 

[p]a = [b]yi + [tv]a 

where tt is assumed to be readily available in B, and so the foregoing 
equation must be expressed as 

[p]h = [b]yi + [Q]xi[7r]B 

which thus proves eq.(2.93a). To prove eq. (2.93b), we simply solve eq.(2.94) 
for TT and apply eq.(2.92) to the equation thus resulting, which readily leads 
to the desired relation. 



Example 2.5.2 If [b]^i = [—1, —1, —1]^ and A and B have the relative 
orientations given in Example 2.5.1, find the position vector, in B, of a 
point P of position vector [p]yi given as in the same example. 



Solution: What we obviously need is [tt]®, which is given in eq.(2.93b). We 
thus compute first the sum inside the parentheses of that equation, i.e.. 



[-b]xi + [p]xi 



2 

2 

2 




FIGURE 2.4. Coordinate frames with different origins. 
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We need further [Q^]b, which can be readily derived from [Q]b. We do 
not have as yet this matrix, but we have which is identical to 

[Q^]b by virtue of Theorem 2.5.2. Therefore, 



■ 0 0 -r 




'2' 




■-2' 


-10 0 




2 


= 


-2 


0 1 0 




1 

CM 
1 




1 

CM 
1 



a result that the reader is invited to verify by inspection. 



2.5.3 Homogeneous Coordinates 

The general coordinate transformation, involving a shift of the origin, is not 
linear, in general, as can be readily realized by virtue of the nonhomoge- 
neous term involved, i.e., the first term of the right-hand side of eq.(2.93a), 
which is independent of p. Such a transformation, nevertheless, can be rep- 
resented in homogeneous form if homogeneous coordinates are introduced. 
These are defined below: Let [p be the coordinate array of a finite point 
P in reference frame Ad. What we mean by a finite point is one whose co- 
ordinates are all finite. We are thus assuming that the point P at hand is 
not at infinity, points at infinity being dealt with later. The homogeneous 
coordinates of P are those in the 4-dimensional array {p}y 4 , defined as 



{p}a4 




(2.95) 



The affine transformation of eq. (2.93a) can now be rewritten in homo- 
geneous-coordinate form as 



{pU = {T}yl{7r}B 



where {T}^i is defined as a 4 x 4 array, i.e.. 



{TU 



[QU [bU 

[o^U 1 



(2.96) 



(2.97) 



The inverse transformation of that defined in eq.(2.97) is derived from 
eq. (2.93a & b), i.e.. 



{T-^b 



[QC]b [-b]B 
[O^h 1 



(2.98) 



Furthermore, homogeneous transformations can be concatenated. In- 
deed, let Afc, for A: = * — 1, *, * + 1, denote three coordinate frames, with 
origins at Ok - Moreover, let Qj_i be the rotation carrying A)_i into an ori- 
entation coinciding with that of A) . If a similar definition for Q j is adopted, 
then Qj denotes the rotation carrying A* into an orientation coinciding with 
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that of First, the case in which all three origins coincide is considered. 
Clearly, 

[p]i = [Qf-i]i-i[p]i-i (2.99) 

[p]i+i = [Qf ]i[p]i = [Qf ]i[Qri]i-i[p]i-i (2.100) 

the inverse relation of that appearing in eq.(2.100) being 

[p]i-l = [Qi-l]i-l[Qi]i[P]i+l (2.101) 



If now the origins do not coincide, let aj_i and a* denote the vectors 
Oj-iOj and OjOj+i, respectively. The homogeneous-coordinate transfor- 
mations {Ti and m i thus arising are obviously 



[ Q-j \i [ 

1 

( 2 . 102 ) 

whereas their inverse transformations are 






[Qi-l ] 
(0^1,: 






{Tyj* 

{Tr'r+i 



Qf-ik 


[Qf- 


1 [ ^i—1 






1 


QlUi 


[Qf 


j-i+l [ 


O^h+i 




1 



(2.103) 

(2.104) 



Hence, the coordinate transformations involved are 



{p}i-i = {Ti-i}i-i{p}i (2.105) 

{p}i-i = {Ti_i}i_i{Ti}i{p}i+i (2.106) 



the corresponding inverse transformations being 

{p}i = {Tyr{pr-i (2.107) 

{p}i+i = {Tj }j+i{p}j = {T^ P }i-i (2.108) 



Now, if P lies at infinity, we can express its homogeneous coordinates in 
a simpler form. To this end, we rewrite expression (2.95) in the form 



{p}m = IIpII 



[e]M 

1/IIpII 



and hence. 



lim {p}a4 

pII^oo 



lim IIpII 

pII^oo 




or 



lim {p}a4 

pII^oo 



lim IIpII 

pII^oo 



[e]M 

0 
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We now define the homogeneous coordinates of a point P lying at infinity 
as the 4-dimensional array appearing in the foregoing expression, i.e., 



{ Poo } M 



[e]M 

0 



(2.109) 



which means that a point at infinity, in homogeneous coordinates, has only 
a direction, given by the unit vector e, but an undefined location. When 
working with objects within the atmosphere of the earth, for example, stars 
can be regarded as lying at infinity, and hence, their location is completely 
specified simply by their longitude and latitude, which suffice to define the 
direction cosines of a unit vector in spherical coordinates. 

On the other hand, a rotation matrix can be regarded as composed of 
three columns, each representing a unit vector, e.g.. 



Q = [ei 62 63 ] 

where the triad {es,}® is orthonormal. We can thus represent { T }^i of 
eq.(2.97) in the form 



{TU 



6i 62 63 b 

0 0 0 1 



( 2 . 110 ) 



thereby concluding that the columns of the 4x4 matrix T represent the 
homogeneous coordinates of a set of corresponding points, the first three 
of which are at infinity. 

Examplo 2.5.3 An ellipsoid is centered at a point Og of position vector 
b, its three axes X , y , and Z defining a coordinate frame B. Moreover, its 
semiaxes have lengths a = 1, 6 = 2, and c = 3, the coordinates of Og in 
a coordinate frame A being [b]^i = [1, 2 , 3]^. Additionally, the direction 
cosines of X are (0.933, 0.067, —0.354), whereas y is perpendicular to b 
and to the unit vector u that is parallel to the X-axis. Find the equation of 
the ellipsoid in A. (This example has relevance in collision- avoidance algo- 
rithms, some of which approximate manipulator links as ellipsoids, thereby 
easing tremendously the computational requirements.) 

Solution: Let u, v, and w be unit vectors parallel to the X-, y-, and ^-axes, 
respectively. Then, 



l^U 



0.933 

0.067 

-0.354 



u X b 



W = U X V 





■ 0.243 ■ 




■ -0.266' 


v]xl = 


-0.843 

0.481 


, [w]yi = 


-0.535 

-0.803 



and hence, 
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from which the rotation matrix Q, rotating the axes of A into orientations 
coinciding with those of B, can be readily represented in A, or in B for that 
matter, as 



[Q]h = [u, V, w]^ = 



0.933 0.243 -0.266 

0.067 -0.843 -0.535 

-0.354 0.481 -0.803 



On the other hand, if the coordinates of a point P in ^ and B are [p]yi = 
[pi, P 2 , and [ttJb = [tti, 7T2, respectively, then the equation of 

the ellipsoid in B is clearly 



B: 



_ 2 . 

22 



''3 

32 



1 



Now, what is needed in order to derive the equation of the ellipsoid in A is 
simply a relation between the coordinates of P in P and those in A. These 
coordinates are related by eq.(2.93b), which requires [ ]g, while we have 

[Q]h- Nevertheless, by virtue of Theorem 2.5.2 



[Q^]b = [Q^U 



0.933 0.067 -0.354 

0.243 -0.843 0.481 

-0.266 -0.535 -0.803 



Hence, 





■ 0.933 


0.067 


-0.354' 


/ 


■-1' 




Pi 


7t]b = 


0.243 


-0.843 


0.481 




-2 


+ 


P2 




-0.266 


-0.535 


-0.803 


V 


-3 




.T3. 



Therefore, 



7Ti = 0.933pi + 0.067p2 ~ 0.354p3 — 0.005 

7T2 = 0.243pi — 0.843p2 + 0.481p3 

7T3 = — 0.266pi — 0.535p2 ~ 0.803p3 + 3.745 

Substitution of the foregoing relations into the ellipsoid equation in B leads 
to 



A: 32.1521pi2 + 7.70235p2^ + 9.17286^3^ - 8.30524pi - 16.0527p2 

— 23.9304p3 + 9.32655pip2 + 9.02784p2P3 ~ 19.9676pip3 + 20.101 = 0 

which is the equation sought, as obtained using computer algebra. 
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2.6 Similarity Transformations 

Transformations of the position vector of points under a change of coor- 
dinate frame involving both a translation of the origin and a rotation of 
the coordinate axes was the main subject of Section 2.5. In this section, we 
study the transformations of components of vectors other than the position 
vector, while extending the concept to the transformation of matrix entries. 
How these transformations take place is the subject of this section. 

What is involved in the present discussion is a change of basis of the 
associated vector spaces, and hence, this is not limited to 3-dimensional 
vector spaces. That is, n-dimensional vector spaces will be studied in this 
section. Moreover, only isomorphisms, i.e., transformations L of the n-di- 
mensional vector space V onto itself will be considered. Let A = {aj}" and 
B = {bj}" be two different bases of the same space V. Hence, any vector 
V of V can be expressed in either of two ways, namely, 

V = Oiai + 02a2 + • • • + (Xn^n (2-111) 

V = /3lbi + /?2b2 + • • • + /?nbn (2.112) 

from which two representations of v are readily derived, namely, 

'ail r /?! ■ 

O 2 /?2 

[v]yi= . , [v]h= . (2.113) 

- _ 

Furthermore, let the two foregoing bases be related by 

bj = ayai + a 2 j-a 2 H \- j = l,...,n (2.114) 

Now, in order to find the relationship between the two representations 
of eq.(2.113), eq.(2.114) is substituted into eq.(2.112), which yields 

V = /3i(aiiai + U2ia2 + • • • + «nian) 

+ /32(ui2ai + 022^2 + • • • + 0,n2^n) 

+ /3n(c*inai + a, 2 „a 2 + • • • + a„„a„) (2.115) 

This can be rearranged to yield 

V = (aii/3i + £112/32 + • • • + Uln/3n)ai 

+ (£* 21 / 3 i + a,22p2 + • • • + 0,2nfi„)a2 



+ (£*nl/3l + Cln2/32 + ' ' ' + /3„)a„ 



(2.116) 
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Comparing eq.(2.116) with eq.(2.111), one readily derives 



[v]yi = [A]^[v]b 



(2.117) 



where 



ail 


£*12 


■ ■ 


£*21 


£*22 


• • to 


- ££nl 


£*n2 


^nn 



(2.118) 



which are the relations sought. Clearly, the inverse relationship of eq.(2.117) 
is 



[v]b = [A 



(2.119) 



Next, let L have the representation in A given below: 





' ^11 


to 


' ' hn 


L]t = 


hi 


^22 


hn 




1 


to 


1 



( 2 . 120 ) 



Now we aim at finding the relationship between [L]^i and [L]g. To this 
end, let w be the image of v under L, i.e.. 



Lv = w (2.121) 

which can be expressed in terms of either A or B as 

[L]yl[v]xi = [w]^ (2.122) 

[L]g[v]g = [w]b (2.123) 

Now we assume that the image vector w of the transformation of eq.(2.121) 
is identical to that of vector v in the range of L, which is not always the 
case. Our assumption is, then, that similar to eq.(2.117), 

[w]^ = [A]^[w]b (2.124) 

Now, substitution of eq.(2.124) into eq.(2.122) yields 

[A]^i[w]b = [L]yi[A]yi[v]B (2.125) 

which can be readily rearranged in the form 

[w]b = [A-1]^[L]^[A]^[v]b (2.126) 

Comparing eq.(2.123) with eq.(2.126) readily leads to 

[L]b= [A-iU[LU[AU 



(2.127) 
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which upon rearrangement, becomes 

[LU=[AU[L]b[A-^U (2.128) 

Relations (2.117), (2.119), (2.127), and (2.128) constitute what are called 
similarity transformations. These are important because they preserve in- 
variant quantities such as the eigenvalues and eigenvectors of matrices, the 
magnitudes of vectors, the angles between vectors, and so on. Indeed, one 
has the following: 

Theorem 2.6.1 The characteristic polynomial of a given nx n matrix re- 
mains unchanged under a similarity transformation. Moreover, the eigen- 
values of two matrix representations of the same nxn linear transformation 
are identical, and if [e]g is an eigenvector of [L]g, then under the sim- 
ilarity transformation (2.128), the csorresponding eigenvector of [h]ji is 
[e]yi = [ A]yi[e]B. 

Proof: From eq.(2.11), the characteristic polynomial of [L]g is 

P(A) =det(A[l]B- [L]b) (2.129) 

which can be rewritten as 

P(A) =det(A[A-i]^[l]^[AU - [A-^U[LU[AU) 

= det([A-i]^(A[lU-[L]^)[AU) 

= det([A^^]^)det(A[l]^ - [L]^)det([ A]^) 

But 

det([A^^]^)det([A]^) = 1 

and hence, the characteristic polynomial of [L]^i is identical to that of 
[L]g. Since both representations have the same characteristic polynomial, 
they have the same eigenvalues. Now, if [e]g is an eigenvector of [L]g 
associated with the eigenvalue A, then 

[L]b[6]b = A[e]g 

Next, eq.(2.127) is substituted into the foregoing equation, which thus leads 
to 

[ ]yl[L]yi[ A]yi[e]B = A[e]g 

Upon rearrangement, this equation becomes 

[L]yt[ A]yi[e]B = A[ A]yi[e]B (2.130) 

whence it is apparent that [ A]^i[e]B is an eigenvector of [L]^i associated 
with the eigenvalue A, q.e.d. 
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Theorem 2.6.2 If [L]^i and [L]g are related by the similarity transfor- 
mation (2.127), then 



[L'^]b=[A-^U[L'^U[AU (2.131) 

for any integer k. 

Proof: This is done by induction. For k = 2, one has 

[L^]^ = [a-^U[lu[au[a-^U[i^U[au 

= [A-^U[L^U[AU 

Now, assume that the proposed relation holds for k = n. Then, 

[L”+1]b^[A-1U[L”U[AU[A-1U[LU[AU 

= [a-1]^[l"+'U[au 

i.e., the relation also holds for A: = n + 1, thereby completing the proof. 

Theorem 2.6.3 The trace of an n x n matrix does not change under a 
similarity transformation. 

Proof: A preliminary relation will be needed: Let [A], [B] and [C] be 
three different n x n matrix arrays, in a given reference frame, that need 
not be indicated with any subscript. Moreover, let ajj, bij, and Cij be the 
components of the said arrays, with indices ranging from 1 to n. Hence, 
using standard index notation, 

tr([A] [B] [C]) = aijbjkCki = hguCkidij = tr([B] [C] [A]) (2.132) 

Taking the trace of both sides of eq.(2.127) and applying the foregoing 
result produces 

tr([L]B) = tr([ A-i]^[L]^[AU) = tr([A]^[A-i U[L]^) = tr([L]^) 

(2.133) 

thereby proving that the trace remains unchanged under a similarity trans- 
formation. 

Example 2.6.1 We consider the equilateral triangle .sketched in Fig. 2.5, 
of .side length equal to 2, with vertices Pi, P 2 , and P 3 , and coordinate 
frames A and B of axes X , Y and X' , Y' , respectively, both with origin at 
the centroid of the triangle. Let P be a 2 x 2 matrix defined by 

P=[Pi P 2 ] 

with Pi denoting the po.sition vector of Pi in a given coordinate frame. Show 
that matrix P does not obey a similarity transformation upon a change of 
frame, and compute its trace in frames A and B to make it apparent that 
this matrix does not comply with the conditions of Theorem 2.6.3. 
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FIGURE 2.5. Two coordinate frames used to represent the position vectors of 
the corners of an equilateral triangle. 



Solution: Prom the figure it is apparent that 



[PU 



Apparently, 



1 0 
-a/3/3 2a/3/3J ’ 



[P]b 



0 1 
2a/3/3 a/3/3 



tr([PU) 




t^tr([P]B) 



3 



The reason why the trace of this matrix did not remain unchanged under 
a coordinate transformation is that the matrix does not obey a similarity 
transformation under a change of coordinates. Indeed, vectors p* change 
as 



[Pi]u = [Q]u[Pi]B 

under a change of coordinates from B to A, with Q denoting the rotation 
carrying A into B. Hence, 



[pU = [QU[p]b 



which is different from the similarity transformation of eq.(2.128). However, 
if we now define 



R = PP^ 



then 

[RU 



1 

V3/3 



V3/3' 
5/3 J ’ 



[R]b 



1 a/3/3 

a/3/3 5/3 



and hence, 

tr([R]B) = ^ 

thereby showing that R does comply with the conditions of Theorem 2.6.3. 
Indeed, under a change of frame, matrix R changes as 



[RU = [PP^]^ = [Q]^[P]B([Q]^[P]Br = [QU[PP]i[Q^U 



which is indeed a similarity transformation. 
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2.7 Invariance Concepts 

From Example 2.6.1 it is apparent that certain properties, like the trace 
of certain square matrices do not change under a coordinate transforma- 
tion. For this reason, a matrix like R of that example is said to be frame- 
invariant, or simply invariant, whereas matrix P of the same example is 
not. In this section, we formally define the concept of invariance and high- 
light its applications and its role in robotics. Let a scalar, a vector, and 
a matrix function of the position vector p be denoted by /(p), f(p) and 
F(p), respectively. The representations of f(p) in two different coordinate 
frames, labelled A and B, will be indicated as [f(p)]yi and [f(p)]g, respec- 
tively, with a similar notation for the representations of F(p). Moreover, 
let the two frames differ both in the location of their origins and in their 
orientations. Additionally, let the proper orthogonal matrix [Q]yi denote 
the rotation of coordinate frame A into B. Then, the scalar function /(p) 
is said to be frame invariant, or invariant for brevity, if 

/([p]b) = Z([P]xi) (2.134) 

Moreover, the vector quantity f is said to be invariant if 

[f]^ = [QUlih (2.135) 

and finally, the matrix quantity F is said to be invariant if 

[F]^ = [Q]^[F]b[Q^]^ (2.136) 

Thus, the difference in origin location becomes irrelevant in this context, 
and hence, will no longer be considered. Prom the foregoing discussion, it 
is clear that the same vector quantity has different components in differ- 
ent coordinate frames; moreover, the same matrix quantity has different 
entries in different coordinate frames. However, certain scalar quantities 
associated with vectors, e.g., the inner product, and matrices, e.g., the ma- 
trix moments, to be defined presently, remain unchanged under a change 
of frame. Additionally, such vector operations as the cross product of two 
vectors are invariant. In fact, the scalar product of two vectors a and b 
remains unchanged under a change of frame, i.e., 

[a]^ [b]^ = [a]g [b]g (2.137) 

Additionally, 

[axb]^ = [Q]^[axb]g (2.138) 

The A:th moment of an n x n matrix T, denoted by Ik, is defined as 
(Leigh, 1968) 

Ifc=tr(T'^), k = Q,l,... (2.139) 

where Iq = tr(l) = n. Now we have 
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Theorem 2.7.1 If the trace of an n x n matrix T is invariant, then so 
are its moments. 

Proof: This is straightforward. Indeed, from Theorem 2.6.2, we have 

[T'^]b=[A-^U[T'^U[AU (2.140) 

Now, let [2k]yi and [Ik]g denote the A:th moment of [T]^ and [T]g, 
respectively. Thus, 

[Ik]g = tr([ A-i]^ [T*^]^ [A] J ^ tr([AU [A-^]^ [T*^] J 

= tr([T*^]J^[IkU 

thereby completing the proof. 

Furthermore, 

Theorem 2.7.2 Annxn matrix has only n linearly independent moments. 



Proof: Let the characteristic polynomial of T be 

P(A) = ao + aiA + • • • + a„_iA"-^ + A" = 0 (2.141) 

Upon application of the Cayley-Hamilton Theorem, eq.(2.141) leads to 

aol + aiT + • • • + a„_iT”-i + T” = 0 (2.142) 

where 1 denotes the n x n identity matrix. 

Now, if we take the trace of both sides of eq.(2.142), and Definition (2.139) 
is recalled, one has 



®o2o + CqTl + • • • + Un-lIn-1 + — 0 

from which it is apparent that I„ can be expressed as a linear combination 
of the first n moments of T, {1^ By simple induction, one can likewise 

prove that the mth moment is dependent upon the first n moments if 
m > n, thereby completing the proof. 

The vector invariants of an n x n matrix are its eigenvectors, which 
have a direct physical significance in the case of symmetric matrices. The 
eigenvalues of these matrices are all real, its eigenvectors being also real and 
mutually orthogonal. Skew-symmetric matrices, in general, need not have 
either real eigenvalues or real eigenvectors. However, if we limit ourselves 
to 3 X 3 skew-symmetric matrices, exactly one of their eigenvalues, and its 
associated eigenvector, are both real. The eigenvalue of interest is 0, and 
the associated vector is the axial vector of the matrix under study. 

It is now apparent that two nx n matrices related by a similarity trans- 
formation have the same set of moments. Now, by virtue of Theorem 2.7.2, 
one may be tempted to think that if two n x n symmetric matrices share 
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their first n moments {1^}"^^, then the two matrices are related by a 
similarity transformation. A simple example will show that this is not true. 
Consider the two matrices A and B given below: 



A 



1 0 
0 1 ’ 



B 



1 2 
2 1 



The two foregoing matrices cannot possibly be related by a similarity trans- 
formation, for the first one is the identity matrix, while the second is not. 
However, the two matrices share the two moments Iq = 2 and Ii = 2. Let 
us now compute the second moments of these matrices: 



tr(A^) 



2, tr(B2) 



tr 



5 4 
4 5 



10 



which are indeed different. Therefore, to test whether two different matri- 
ces represent the same linear transformation, and hence, are related by a 
similarity transformation, we must verify that they share the same set of 
n + 1 moments {1^ }". In fact, since all n x n matrices share the same 
zeroth moment Iq = n, only the n moments { Ij, }" need be tested for a 
similarity verification. That is, if two n x n matrices share the same n mo- 
ments {Ifc }", then they represent the same linear transformation, albeit 
in different coordinate frames. 

The foregoing discussion does not apply, in general, to nonsymmetric 
matrices, for these matrices are not fully characterized by their eigenvalues. 
For example, consider the matrix 



Its two first moments are Iq =2, 1i = tr(A) = 2, which happen to be the 
first two moments of the 2x2 identity matrix as well. However, while the 
identity matrix leaves all 2-dimensional vectors unchanged, matrix A does 
not. 

Now, if two symmetric matrices, say A and B, represent the same trans- 
formation, they are related by a similarity transformation, i.e., a nonsin- 
gular matrix T exists such that 

B = T ^AT 



Given A and T, then, finding B is trivial, a similar statement holding 
if B and T are given; however, if A and B are given, finding T is more 
difficult. The latter problem occurs sometimes in robotics in the context of 
calibration, to be discussed in Subsection 2.7.1. 

Example 2.7.1 Two symmetric matrices are displayed below. Find out 
whether they are related by a similarity transformation. 





'1 


0 


r 




'1 


0 


0 ■ 


A = 


0 


1 


0 


, B = 


0 


2 


-1 




1 


0 


2 




0 


-1 


1 
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Solution: The traces of the two matrices are apparently identical, namely, 
4. Now we have to verify whether their second and third moments are also 
identical. To do this, we need the square and the cube of the two matrices, 
from which we then compute their traces. Thus, from 





'2 


0 3' 




A 


0 


0 ■ 


A2 = 


0 


1 0 


, R2 = 


0 


5 


-3 




3 


0 5 




0 


-3 


2 


we readily obtain 




tr(A^ 


) = tr(R2) = 


= 8 






Moreover, 


'5 


0 8 




■f 


0 


0 ■ 


A3 = 


0 


1 0 


, R3 = 


0 


13 


-8 




8 


0 13 




0 


-8 


5 



whence 



tr(A3) = tr(B3 



19 



Therefore, the two matrices are related by a similarity transformation. 
Hence, they represent the same linear transformation. 



Example 2.7.2 Same as Example 2.7.1, for the two matrices displayed 
below: 





■f 


0 


2' 




■f 


1 


1' 


A = 


0 


1 


0 


, B = 


1 


1 


0 




2 


0 


0 




1 


0 


0 



Solution: As in the previous example, the traces of these matrices are iden- 
tical, i.e., 2. However, tr(A^) = 10, while tr(B^) = 6. We thus conclude 
that the two matrices cannot be related by a similarity transformation. 

2.7.1 Applications to Redundant Sensing 

A sensor, such as a camera or a range finder, is often mounted on a robotic 
end-effector to determine the pose — i.e., the position and orientation, as 
defined in Subsection 3.2.3 — of an object. If redundant sensors are intro- 
duced, and we attach frames A and B to each of these, then each sensor 
can be used to determine the orientation of the end-effector with respect to 
a reference configuration. This is a simple task, for all that is needed is to 
measure the rotation R that each of the foregoing frames underwent from 
the reference configuration, in which these frames are denoted by Aq and 
Bq, respectively. Let us assume that these measurements produce the or- 
thogonal matrices A and B, representing R in A and B, respectively. With 
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this information we would like to determine the relative orientation Q of 
frame B with respect to frame A, a problem that is called here instrument 
calibration. 

We thus have A = [R]y^ and B = [R]g, and hence, the algebraic 
problem at hand consists in determining [Q]y^ or equivalently, [Q]g. The 
former can be obtained from the similarity transformation of eq.(2.136), 
which leads to 

A= [Q]^B[Q^]^ 

or 

A[QU = [QUB 

This problem could be solved if we had three invariant vectors associated 
with each of the two matrices A and B. Then, each corresponding pair of 
vectors of these triads would be related by eq.(2.135), thereby obtaining 
three such vector equations that should be sufficient to compute the nine 
components of the matrix Q rotating frame A into B. However, since A 
and B are orthogonal matrices, they admit only one real invariant vector, 
namely, their axial vector, and we are short of two vector equations. We 
thus need two more invariant vectors, represented in both A and B, to 
determine Q. The obvious way of obtaining one additional vector in each 
frame is to take not one, but two measurements of the orientation of Aq 
and Bq with respect to A and B, respectively. Let the matrices representing 
these orientations be given, in each of the two coordinate frames, by Aj 
and Bj, for * = 1,2. Moreover, let a* and bj, for * = 1,2, be the axial 
vectors of matrices Aj and Bj, respectively. 

Now we have two possibilities: (*) neither of ai and a 2 and, consequently, 
neither of bi and b 2 , is zero; and (n) at least one of ai and a 2 , and 
consequently, the corresponding vector of the {bi, b 2 } pair, vanishes. In 
the first case, nothing prevents us from computing a third vector of each 
set, namely, 

aa = aixa 2 , ba = bi x b 2 (2.143) 

In the second case, however, we have two more possibilities, i.e., the angle 
of rotation of that orthogonal matrix, Ai or A 2 , whose axial vector vanishes 
is either 0 or tt. If the foregoing angle vanishes, then A underwent a pure 
translation from Ao, the same holding, of course, for B and Bq. This means 
that the corresponding measurement becomes useless for our purposes, and 
a new measurement is needed, involving a rotation. If, on the other hand, 
the same angle is tt, then the associated rotation is symmetric and the unit 
vector e parallel to its axis can be determined from eq.(2.49) in both A 
and B. This unit vector, then, would play the role of the vanishing axial 
vector, and we would thus end up, in any event, with two pairs of nonzero 
vectors, {a* and {bj . As a consequence, we can always find two triads 
of nonzero vectors, { a* and { bj }®, that are related by 

aj = [QUbj, for *=1,2, 3 (2.144) 
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The problem at hand now reduces to computing [ Q from eq.(2.144). In 
order to perform this computation, we write the three foregoing equations 
in matrix form, namely, 

E=[Q]^F (2.145) 

with E and F defined as 



E=[ai a 2 as], F=[bi b 2 ba ] (2.146) 

Now, by virtue of the form in which the two vector triads were defined, 
none of the two above matrices is singular, and hence, we have 

[Q]^ = EF-i (2.147) 

Moreover, note that the inverse of F can be expressed in terms of its 
columns explicitly, without introducing components, if the concept of re- 
ciprocal bases is recalled (Brand, 1965). Thus, 

^(b2 X bg)^^ 

(bg X bi)^ 

(bi X bg)^ 



F-^ = — 



A = bl X bg • bg 



(2.148) 



Therefore, 



1 



[QU = ^[^i(b2 X bg)-' +ag(b 3 X bl)-' +ag(bi X bg)-' ] (2.149) 

thereby completing the computation of [QJ^i directly and with simple vec- 
tor operations. 



Example 2.7.3 (Hand-Eye Calibration) Determine the relative orien- 
tation of a frame B attached to a camera mounted on a robot end- effector, 
with respect to a frame A fixed to the latter, as shown in Fig. 2. 6. It is as- 
sumed that two measurements of the orientation of the two frames with re- 
spect to frames Mq and Bg in the reference configuration of the end- effector 
are available. These measurements produce the orientation matrices Ai of 
the frame fixed to the camera and Bj of the frame fixed to the end-effector, 
for i = 1,2. The numerical data of this example are given below: 





'-0.92592593 


-0.37037037 


-0.07407407 


Ai = 


0.28148148 


-0.80740741 


0.51851852 




-0.25185185 


0.45925926 


0.85185185 




'-0.83134406 


0.02335236 


-0.55526725' 


Ag = 


-0.52153607 


0.31240270 


0.79398028 




0.19200830 


0.94969269 


-0.24753503 




'-0.90268482 


0.10343126 


-0.41768659 


Bl = 


0.38511568 


0.62720266 


-0.67698060 




0.19195318 


-0.77195777 


-0.60599932 




'-0.73851280 


-0.54317226 


0.39945305 


Bg = 


-0.45524951 


0.83872293 


0.29881721 




-0.49733966 


0.03882952 


-0.86668653 
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(I) 





j4q 



FIGURE 2.6. Measuring the orientation of a camera-fixed coordinate frame with 
respect to a frame fixed to a robotic end-effector. 

Solution: Shin and Ahmad (1987) formulated this problem in the form 
of a matrix linear homogeneous equation, while Chou and Kamel (1988) 
solved the same problem using quaternions and very cumbersome numeri- 
cal methods that involve singular- value computations. The latter require an 
iterative procedure within a Newton-Raphson method, itself iterative, for 
nonlinear-equation solving. Other attempts to solve the same problem have 
been reported in the literature, but these also resorted to extremely com- 
plicated numerical procedures for nonlinear-equation solving (Chou and 
Kamel, 1991). More recently, Horaud and Dornaika (1995) proposed a more 
concise method based on quaternions, a.k.a. Euler- Rodrigues parameters, 
that nevertheless is computationally costlier than the method we use here. 
The approach outlined in this subsection is essentially the same as that 
proposed earlier (Angeles, 1989), although here we have adopted a simpler 
procedure than that of the foregoing reference. 

First, the vector of matrix Aj, represented by a*, and the vector of matrix 
Bj, represented by bj, for * = 1, 2, are computed from simple differences of 
the off-diagonal entries of the foregoing matrices, followed by a division by 
2 of all the entries thus resulting, which yields 





■ -0.02962963' 




■ 0.07784121 ■ 


ai = 


0.08888889 


, ^2 — 


-0.37363778 




0.32592593 




-0.27244422 
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■ -0.04748859' 




■ -0.12999385' 


bi = 


-0.30481989 


, ba 


0.44869636 




0.14084221 




0.04396138 



In the calculations below, 16 digits were used, but only eight are dis- 
played. Furthermore, with the foregoing vectors, we compute aa and ba 
from cross products, thus obtaining 



aa 



0.09756097 

0.01730293 

0.00415020 



ba 



-0.07655343 

-0.01622096 

-0.06091842 



Furthermore, A is obtained as 

A = 0.00983460 

while the individual rank-one matrices inside the brackets of eq.(2.149) are 
calculated as 



ai(ba 


X ba; 


f = 


0.00078822 

-0.00236467 

-0.00867044 


0.00033435 

-0.00100306 

-0.00367788 


-0.00107955 

0.00323866 

0.01187508 


aa(ba x bi^ 


f = 


'-0.00162359 

0.00779175 

0.00568148 


0.00106467 

-0.00510945 

-0.00372564 


0.00175680 

-0.00843102 

-0.00614762 


aa(bi 


X ba; 


f = 


'-0.00746863 

-0.00132460 

-0.00031771 


-0.00158253 

-0.00028067 

-0.00006732 


-0.00594326 

-0.00105407 

-0.00025282 



whence Q in the A frame is readily obtained as 



[QU 



-0.84436553 

0.41714750 

-0.33622873 



-0.01865909 

-0.65007032 

-0.75964911 



-0.53545750 

-0.63514856 

0.55667078 



thereby completing the desired computation. 




3 

Fundamentals of Rigid-Body 
Mechanics 



3.1 Introduction 

The purpose of this chapter is to lay down the foundations of the kineto- 
statics and dynamics of rigid bodies, as needed in the study of multibody 
mechanical systems. With this background, we study the kinetostatics and 
dynamics of robotic manipulators of the serial type in Chapters 4 and 6, 
respectively, while devoting Chapter 5 to the study of trajectory planning. 
The latter requires, additionally, the background of Chapter 4. A special 
feature of this chapter is the study of the relations between the angular 
velocity of a rigid body and the time-rates of change of the various sets 
of rotation invariants introduced in Chapter 2. Similar relations between 
the angular acceleration and the second time-derivatives of the rotation 
invariants are also recalled, the corresponding derivations being outlined in 
Appendix A. 

Furthermore, an introduction to the very useful analysis tool known as 
screw theory (Roth, 1984) is included. In this context, the concepts of twist 
and wrench are introduced, which prove in subsequent chapters to be ex- 
tremely useful in deriving the kinematic and static, i.e., the kinetostatic, 
relations among the various bodies of multibody mechanical systems. 
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3.2 General Rigid-Body Motion and Its Associated 
Screw 

In this section, we analyze the general motion of a rigid body. Thus, let A 
and P be two points of the same rigid body B, the former being a particular 
reference point, whereas the latter is an arbitrary point of B. Moreover, the 
position vector of point A in the original configuration is a, and the position 
vector of the same point in the displaced configuration, denoted by A! , is 
a'. Similarly, the position vector of point P in the original configuration is 
p, while in the displaced configuration, denoted by P', its position vector is 
p'. Furthermore, p' is to be determined, while a, a', and p are given, along 
with the rotation matrix Q. Vector p — a can be considered to undergo a 
rotation Q about point A throughout the motion taking the body from the 
original to the final configuration. Since vector p — a is mapped into p' — a' 
under the above rotation, one can write 

p'-a' = Q(p-a) (3.1) 

and hence 

p' = a' + Q(p-a) (3.2) 

which is the relationship sought. Moreover, let and dp denote the dis- 
placements of A and P, respectively, i.e., 

d^ = a' — a, dp = p' — p (3-3) 

From eqs.(3.2) and (3.3) one can readily obtain an expression for dp, 
namely. 



dp = a' - p + Q(p - a) 

= a' - a - p + Q(p - a) + a (3.4) 

= dA + (Q - l)(p - a) (3.5) 

What eq.(3.5) states is that the displacement of an arbitrary point P of 
a rigid body whose position vector in an original configuration is p is de- 
termined by the displacement of one certain point A and the concomitant 
rotation Q. Clearly, once the displacement of P is known, its position vec- 
tor p' can be readily determined. An interesting result in connection with 
the foregoing discussion is summarized below: 

Theorem 3.2.1 The component of the displacements of all the points of 
a rigid body undergoing a general motion along the axis of the underlying 
rotation is a constant. 

Proof: Multiply both sides of eq.(3.5) by e^, the unit vector parallel to the 
axis of the rotation represented by Q, thereby obtaining 

e^dp = e^dA + e^(Q - l)(p - a) 
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Now, the second term of the right-hand side of the above equation vanishes 
because Qe = e, and hence, Q^e = e, by hypothesis, the said equation 
thus leading to 

e^dp = = do (3-6) 

thereby showing that the displacements of all points of the body have the 
same projection do onto the axis of rotation, q.e.d. 

As a consequence of the foregoing result, we have the classical Mozzi- 
Chasles Theorem (Mozzi, 1763; Chasles, 1830; Ceccarelli, 1995), namely. 

Theorem 3.2.2 (Mozzi, 1763; Chasles, 1830) Given a rigid body un- 
dergoing a general motion, a set of its points located on a line C undergo 
identical displacements of minimum magnitude. Moreover, line C and the 
minimum-magnitude displacement are parallel to the axis of the rotation 
involved. 

Proof: The proof is straightforward in light of Theorem 3.2.1, which al- 
lows us to express the displacement of an arbitrary point P as the sum of 
two orthogonal components, namely, one parallel to the axis of rotation, 
independent of P and denoted by d||, and one perpendicular to this axis, 
denoted by dy, i.e., 

dp = d|| + dy (3.7a) 

where 

d|| = ee^dp = doe, dp = (1 — ee^)dp (3.7b) 

and clearly, do is a constant that is defined as in eq.(3.6), while d|| and dp 
are mutually orthogonal. Indeed, 

d|| • dp = doe^(l ~ ee^)dp = do{e'^ — e^)dp = 0 

Now, by virtue of the orthogonality of the two components of dp, it is 
apparent that 

lldpf = ||d||f + ||dpf =d2 + ||d^||2 

for the displacement dp of any point of the body. Now, in order to minimize 
||dp|| we have to make ||dp||, and hence, dp itself, equal to zero, i.e., we 
must have dp parallel to e: 

dp = ae 

for a certain scalar a. That is, the displacements of minimum magnitude 
of the body under study are parallel to the axis of Q, thereby proving the 
first part of the Mozzi-Chasles Theorem. The second part is also readily 
proven by noticing that if P* is a point of minimum magnitude of position 
vector p* , its component perpendicular to the axis of rotation must vanish, 
and hence. 



dp = (1 — ee^)dp* 

= (1 - ee^)dA + (1 - ee^)(Q - l)(p* - a) = 0 
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Upon expansion of the above expression for d^, the foregoing equation 
leads to 

(1 - ee^)dA + (Q - l)(p* - a) = 0 

Now it is apparent that if we define a line C passing through P* and parallel 
to e, then the position vector p* + Ae of any of its points P satisfies the 
foregoing equation. As a consequence, all points of minimum magnitude lie 
in a line parallel to the axis of rotation of Q, q.e.d. 

An important implication of the foregoing theorem is that a rigid body 
can attain an arbitrary configuration from a given original one, following a 
screw-like motion of axis C and pitch p, the latter being defined presently. 
Thus, it seems appropriate to call C the screw axis of the rigid-body motion. 

Note that do, as defined in eq.(3.6), is an invariant of the motion at hand. 
Thus, associated with a rigid-body motion, one can then define a screw of 
axis C and pitch p. Of course, the pitch is defined as 



do 





or 



P = 



27rdo 



(3.8) 



which has units of m/rad or, correspondingly, of m/turn. Moreover, the 
angle <f> of the rotation involved can be regarded as one more feature of this 
motion. This angle is, in fact, the amplitude associated with the said motion. 
We will come across screws in discussing velocities and forces acting on rigid 
bodies, along with their pitches and amplitudes. Thus, it is convenient to 
introduce this concept at this stage. 



3.2.1 The Screw of a Rigid-Body Motion 

The screw axis C is totally specified by a given point Pq of T that can be 
defined, for example, as that lying closest to the origin, and a unit vector 
e defining its direction. Expressions for the position vector of Pq, po, in 
terms of a, a' and Q, are derived below: 

If Pq is defined as above, i.e., as the point of C lying closest to the origin, 
then, obviously, po is perpendicular to e, i.e., 

e^Po = 0 (3.9) 

Moreover, the displacement do of Pq is parallel to the vector of Q, and 
hence, is identical to d|| defined in eq.(3.7b), i.e., it satisfies 

(Q - l)do = 0 

where do is given as in eq.(3.5), namely, as 

do = dA + (Q - l)(po - a) 

Now, since do is identical to d||, we have, from eq.(3.7b), 
dA + (Q - l)(Po - a) = d|| = ee^do 



(3.10a) 
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But from Theorem 3.2.1, 
and so 



e^do = e^dA 



dA + (Q - l)(Po - a) = ee^dA 
or after rearranging terms, 

(Q - l)po = (Q - l)a - (1 - ee^)dA 



(3.10b) 



Furthermore, in order to find an expression for po, eq.(3.9) is adjoined to 
eq.(3.10b), thereby obtaining 



Apo = b 



(3.11) 



where A is a 4 x 3 matrix and b is a 4-dimensional vector, being given by 



A = 



[Q-i] 


K — 


(Q - l)a - (1 - ee^)dA 




? D — 


0 



(3.12) 



Equation (3.11) cannot be solved for po directly, because A is not a square 
matrix. In fact, that equation represents an overdetermined system of four 
equations and three unknowns. Thus, in general, that system does not 
admit a solution. However, the four equations are compatible, and hence 
in this particular case, a solution of that equation, which turns out to be 
unique^ can be determined. In fact, if both sides of eq.(3.11) are multiplied 
from the left by A^, we have 



A^Apo = A^b 



(3.13) 



Moreover, if the product A^A, which is a 3 x 3 matrix, is invertible, then 
Po can be computed from eq.(3.13). In fact, the said product is not only 
invertible, but also admits an inverse that is rather simple to derive, as 
shown below. Now the rotation matrix Q is recalled in terms of its natural 
invariants, namely, the unit vector e parallel to its axis of rotation and the 
angle of rotation <f> about this axis, as given in eq.(2.48), reproduced below 
for quick reference: 



Q = ee^ + cos (f){l — ee^) + sin <))E 



where 1 represents the 3x3 identity matrix and E the cross-product ma- 
trix of e, as introduced in eq.(2.37). Further, eq.(2.48) is substituted into 
eq.(3.12), which yields 

A^A = 2(1 — cos()))l — (1 — 2 cos (3-14) 



It is now apparent that the foregoing product is a linear combination of 1 
and ee^. This suggests that its inverse is very likely to be a linear com- 
bination of these two matrices as well. If this is in fact true, then one can 
write 

(A^A)-i = al + l3ee^ 



(3.15) 
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coefficients a and /3 being determined from the condition that the product 
of A^A by its inverse should be 1, which yields 



a 



1 

2(1 — cos (fi) ’ 



/? 



1—2 cos 4> 
2(1 — cos (j)) 



(3.16) 



and hence, 



(A^A) 



1 

2(1 — cos (j)) 



1 + 



1—2 cos (b rp 

G©"^ 

2(1 — cos 4>) 



(3.17) 



On the other hand, 



A^b=(Q-l)^[(Q-l)a-dA] (3.18) 



Upon solving eq.(3.13) for po and substituting relations (3.17) and (3.18) 
into the expression thus resulting, one finally obtains 



Po 



(Q-l)^(Qa-aQ 
2(1 — cos 4>) 



for ()) yt 0 



(3.19) 



We have thus defined a line C of the rigid body under study that is 
completely defined by its point Pq of position vector po and a unit vector 
e determining its direction. Moreover, we have already defined the pitch of 
the associated motion, eq.(3.8). The line thus defined, along with the pitch, 
determines the screw of the motion under study. 



3.2.2 The Plucker Coordinates of a Line 

Alternatively, the screw axis, and any line for that matter, can be defined 
more conveniently by its Plucker coordinates. In motivating this concept, 
we recall the equation of a line C passing through two points Pi and P 2 of 
position vectors pi and p 2 , as shown in Fig. 3.1. 

If point P lies in £, then, it must be collinear with Pi and P 2 , a property 
that is expressed as 



(P2 -Pl) X (p-pi) =0 




FIGURE 3.1. A line L passing through two points. 
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or upon expansion, 

(P2 -Pi) X p + pi X (p2 -pi) = 0 (3.20) 

If we now introduce the cross-product matrices Pi and P2 of vectors pi 
and p2 in the above equation, we have an alternative expression for the 
equation of the line, namely, 

(P 2 - Pl)p + Pi X (p2 - Pi) = 0 

The above equation can be regarded as a linear equation in the homoge- 
neous coordinates of point P, namely, 

[P 2 -P 1 Pix(p 2 -pi)] ^ =0 (3.21) 

It is now apparent that the line is defined completely by two vectors, the 
difference p2 — pi, or its cross-product matrix for that matter, and the 
cross product pi x (p2 — Pi). We will thus define a 6 -dimensional array 
containing these two vectors, namely. 



Ic = 



P2 -Pl 

Pi X (P2 - Pl)_ 



(3.22) 



whose six scalar entries are the Pliicker coordinates of C. Moreover, if we 
let 

P2 - Pl 

e=- -, n = pixe 3.23 

l|P2 - Pill 

then we can write 

7£ = IIP2-Pi|| ® 

The six scalar entries of the above array are the normalized Pliicker coor- 
dinates of C. Vector e determines the direction of £, while n determines 
its location; n can be interpreted as the moment of a unit force parallel to 
e and of line of action C. Hence, n is called the moment of C. Henceforth, 
only the normalized Pliicker coordinates of lines will be used. For brevity, 
we will refer to these simply as the Pliicker coordinates of the line under 
study. The Pliicker coordinates thus defined will be thus stored in a Pliicker 
array kc in the form 

(3.24) 

where for conciseness, we have dropped the subscript £, while assuming 
that the line under discussion is self-evident. 

Note, however, that the six components of the Pliicker array, i.e., the 
Pliicker coordinates of line £, are not independent, for they obey 




ee=l, ne = 0 



(3.25) 
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and hence, any line C has only four independent Pliicker coordinates. In the 
foregoing paragraphs, we have talked about the Pliicker array of a line, and 
not about the Pliicker vector; the reason for this distinction is given below. 
The set of Pliicker arrays is a clear example of an array of real numbers 
not constituting a vector space. What disables Pliicker arrays from being 
vectors are the two constraints that their components must satisfy, namely, 
(*) the sum of the squares of the first three components of a Pliicker array 
is unity, and (m) the unit vector of a line is normal to the moment of the 
line. Nevertheless, we can perform with Pliicker arrays certain operations 
that pertain to vectors, as long as we keep in mind the essential differences. 
For example, we can multiply Pliicker arrays by matrices of the suitable 
dimension, with entries having appropriate units, as we will show presently. 

It must be pointed out that a Pliicker array is dependent upon the loca- 
tion of the point with respect to which the moment of the line is measured. 
Indeed, let ka and kb denote the Pliicker arrays of the same line C when 
its moment is measured at points A and B, respectively. Moreover, this 
line passes through a point P of position vector p for a particular origin 
O. Now, let the moment of C with respect to A and B be denoted by ua 
and iiB, respectively, i.e.. 



riA = (p - a) X e, n_e = (p - b) x e 



and hence, 



Obviously, 



Ka = 



e 

Ha 



Kb = 



e 

ns 



iiB — riA = (a — b) X e 



i.e.. 



Kb = 

which can be rewritten as 



riA + (a — b) X e 



Kb = UfCA 

with the 6x6 matrix U defined as 



U = 



1 o 

A-B 1 



(3.26) 

(3.27) 

(3.28) 

(3.29) 

(3.30) 

(3.31) 



while A and B are, respectively, the cross-product matrices of vectors a 
and b, and O denotes the 3x3 zero matrix. Given the lower-triangular 
structure of matrix U, its determinant is simply the product of its diagonal 
entries, which are all unity. Hence, 



det(U) = 1 



(3.32) 
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U thus belonging to the unirnodular group of 6 x 6 matrices. These matrices 
are rather simple to invert. In fact, as one can readily prove, 



u-i 



1 o 

B - A 1 



(3.33) 



Relation (3.30) can then be called the Plucker- coordinate transfer formula. 
Note that upon multiplication of both sides of eq.(3.28) by (a — b), 

(a — b)^riB = (a — b)^riA (3.34) 



and hence, the moments of the same line C with respect to two points are 
not independent, for they have the same component along the line joining 
the two points. 

A special case of a line, of interest in kinematics, is a line at infinity. 
This is a line with undefined orientation, but with a defined direction of its 
moment; this moment is, moreover, independent of the point with respect 
to which it is measured. Very informally, the Plucker coordinates of a line at 
infinity can be derived from the general expression, eq.(3.24), if we rewrite 
it in the form 



K 



l|n|| 



e/||n|| 

n/||n|| 



where clearly n/||n|| is a unit vector; henceforth, this vector will be denoted 
by f. Now let us take the limit of the above expression as P goes to infinity, 
i.e., when ||p|| ^ oo, and consequently, as ||n|| ^ oo. Thus, 



whence 



lim K 



lim 



n 



lim 



e/||n|| 

f 



lim K 

nil — >-oo 



lim ||n|| 



0 

f 



The 6-dimensional array appearing in the above equation is defined as the 
Plucker array of a line at infinity, Koo, namely. 



^oo 



0 

f 



(3.35) 



Note that a line at infinity of unit moment f can be thought of as being 
a line lying in a plane perpendicular to the unit vector f, but otherwise 
with an indefinite location in the plane, except that it is an infinitely large 
distance from the origin. Thus, lines at infinity vary only in the orientation 
of the plane in which they lie. 
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3.2.3 The Pose of a Rigid Body 

A possible form of describing a general rigid-body motion, then, is through 
a set of eight real numbers, namely, the six Pliicker coordinates of its screw 
axis, its pitch, and its amplitude, i.e., its angle. Hence, a rigid-body mo- 
tion is fully described by six independent parameters. Moreover, the pitch 
can attain values from — oo to +oo. Alternatively, a rigid-body motion can 
be described by seven dependent parameters as follows: four invariants of 
the concomitant rotation — the linear invariants, the natural invariants, or 
the Euler-Rodrigues parameters, introduced in Section 2.3 — and the three 
components of the displacement of an arbitrary point. Since those invari- 
ants are not independent, but subject to one constraint, this description 
consistently involves six independent parameters. Thus, let a rigid body 
undergo a general motion of rotation Q and displacement d from a refer- 
ence configuration Cq. If in the new configuration C a landmark point A of 
the body has a position vector a, then the pose array, or simply the pose, 
s of the body, is defined as a 7-dimensional array, namely. 



q 

qo 

dA 



(3.36) 



where the 3-dimensional vector q and the scalar qo are any four invariants 
of Q. For example, if these are the Euler-Rodrigues parameters, then 



q = sm(-)e. 



qo = cos( 






If alternatively, we work with the linear invariants, then 
q = (sin 4>)e, qo = cos <f> 

and, of course, if we work instead with the natural invariants, then 



q = e, qo = (f> 



In the first two cases, the constraint mentioned above is 



l|qf + 9^ = 1 


(3.37) 


In the last case, the constraint is simply 

l|ef = 1 


(3.38) 



An important problem in kinematics is the computation of the screw pa- 
rameters, i.e., the components of s, as given in eq.(3.36), from coordinate 
measurements over a certain finite set of points. Prom the foregoing discus- 
sion, it is clear that the computation of the attitude of a rigid body, given 
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by matrix Q or its invariants, is crucial in solving this problem. Moreover, 
besides its theoretical importance, this problem, known as pose estimation, 
has also practical relevance. Shown in Fig. 3.2 is the helmet-mounted dis- 
play system used in flight simulators. The helmet is supplied with a set of 
LEDs (light-emitting diodes) that emit infrared light signals at different 
frequencies each. These signals are then picked up by two cameras, from 
whose images the Cartesian coordinates of the LEDs centers are inferred. 
With these coordinates and knowledge of the LED pattern, the attitude 
of the pilot’s head is determined from the rotation matrix Q. Moreover, 
with this information and that provided via sensors mounted on the lenses, 
the position of the center of the pupil of the pilot’s eyes is then estimated. 
This position, then, indicates on which part of his or her visual field the 
pilot’s eyes are focusing. In this way, a high-resolution graphics monitor 
synthesizes the image that the pilot would be viewing with a high level of 
detail. The rest of the visual field is rendered as a rather blurred image, in 
order to allocate computer resources where it really matters. 

A straightforward method of computing the screw parameters consists 
of regarding the motion as follows: Choose a certain point A of the body. 




FIGURE 3.2. Helmet-mounted display system (courtesy of CAE Electronics Ltd., 
St. -Laurent, Quebec, Canada.) 




82 



3. Fundamentals of Rigid-Body Mechanics 





FIGURE 3.3. Decomposition of the displacement of a rigid body. 



of position vector a, and track it as the body moves to a displaced config- 
uration, at which point A moves to A! , of position vector a'. Assume that 
the body reaches the displaced configuration B' , passing through an inter- 
mediate one B" , which is attained by pure translation. Next, configuration 
B' is reached by rotating the body about point A! , as indicated in Fig. 3 . 3 . 

Matrix Q can now be readily determined. To do this, define three points 
of the body. Pi, P2, and P3, in such a way that the three vectors defined 
below are orthonormal and form a right-hand system: 

ei = API, 62 = AP2, 63 = AP3 ( 3 . 39 ) 

ei-ej=Sij, *,j = 1 , 2 , 3 , 63 = 61X62 ( 3 . 40 ) 



where Sij is the Kronecker delta, defined as 1 if * = j and 0 otherwise. Now, 
let the set {oj}® be labelled {o'}® and jo"}® in configurations B' and B” , 
respectively. Moreover, let qij denote the entries of the matrix representa- 
tion of the rotation Q in a frame X, Y, Z with origin at A and such that 
the foregoing axes are parallel to vectors ei, 62, and 63, respectively. It is 
clear, from Definition 2 . 2 . 1 , that 





qij = 


6i • 6^. 




ei 


•e'l 


61 • e'2 


61-6(5 


62 


•e'l 


62 -6^ 


62-6(5 


63 


•e'l 


63 -6^ 


63-6(5 



( 3 . 41 ) 



( 3 . 42 ) 



Note that all Oj and e' appearing in eq.( 3 . 42 ) must be represented in the 
same coordinate frame. Once Q is determined, computing the remaining 
screw parameters is straightforward. One can use, for example, eq.( 3 . 19 ) to 
determine the point of the screw axis that lies closest to the origin, which 
would thus allow one to compute the Pliicker coordinates of the screw axis. 
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3.3 Rotation of a Rigid Body About a Fixed Point 

In this section, the motion of a rigid body having a point fixed is analyzed. 
This motion is fully described by a rotation matrix Q that is proper or- 
thogonal. Now, Q will be assumed to be a smooth function of time, and 
hence, the position vector of a point P in an original configuration, denoted 
here by po, is mapped smoothly into a new vector p(t), namely, 

p(t) = Q(t)po (3.43) 

The velocity of P is computed by differentiating both sides of eq.(3.43) 
with respect to time, thus obtaining 

p(t) = Q(t)po (3.44) 

which is not a very useful expression, because it requires knowledge of the 
original position of P. A more useful expression can be derived if eq.(3.43) is 
solved for po and the expression thus resulting is substituted into eq.(3.44), 
which yields 

P = QQ^P (3.45) 

where the argument t has been dropped because all quantities are now 
time- varying, and hence, this argument is self-evident. The product QQ^ 
is known as the angular-velocity matrix of the rigid-body motion and is 
denoted by i.e., 

n = QQ^ (3.46) 

As a consequence of the orthogonality of Q, one has a basic result, 
namely. 

Theorem 3.3.1 The angular-velocity matrix is skew symmetric. 

In order to derive the angular-velocity vector of a rigid-body motion, 
we recall the concept of axial vector, or simply vector, of a 3 x 3 matrix, 
introduced in Subsection 2.3.3. Thus, the angular- velocity vector u> of the 
rigid-body motion under study is defined as the vector of Vl, i.e., 

u) = vect(ri) (3.47) 

and hence, eq.(3.45) can be written as 

p = rip = tu X p (3.48) 

from which it is apparent that the velocity of any point P of a body moving 
with a point O fixed is perpendicular to line OP. 
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3.4 General Instantaneous Motion of a Rigid Body 

If a rigid body now undergoes the most general motion, none of its points 
remains fixed, and the position vector of any of these, P, in a displaced con- 
figuration is given by eq.(3.2). Let ao and po denote the position vectors 
of points A and P of Section 3.2, respectively, in the reference configura- 
tion Co, a(t) and p(t) being the position vectors of the same points in the 
displaced configuration C. Moreover, if Q(t) denotes the rotation matrix, 
then 

p(t) = a(t) + Q(t)(po -ao) (3.49) 

Now, the velocity of P is computed by differentiating both sides of eq.(3.49) 
with respect to time, thus obtaining 

p(t) = a(t) + Q(t)(po -ao) (3.50) 

which again, as expression (3.50), is not very useful, for it requires the 
values of the position vectors of A and P in the original configuration. 
However, if eq.(3.49) is solved for po — ao and the expression thus resulting 
is substituted into eq.(3.50), we obtain 



p = a+r2(p — a) (3.51) 

or in terms of the angular- velocity vector, 

p = a + tux(p — a) (3.52) 

where the argument t has been dropped for brevity but is implicit, since all 
variables of the foregoing equation are now functions of time. Furthermore, 
from eq.(3.52), it is apparent that the result below holds: 

(p — a) • (p — a) = 0 (3.53) 

which can be summarized as 

Theorem 3.4.1 The relative velocity of two points of the same rigid body 
is perpendicular to the line joining them. 

Moreover, similar to the outcome of Theorem 3.2.1, one now has an ad- 
ditional result that is derived upon dot-multiplying both sides of eq.(3.52) 
by tu, namely, 

tu • p = tu • a = constant 

and hence. 

Corollary 3.4.1 The projections of the velocities of all the points of a 
rigid body onto the angular-velocity vector are identical. 

Furthermore, similar to the Mozzi-Chasles Theorem, we have now 




3.4 General Instantaneous Motion of a Rigid Body 85 



Theorem 3.4.2 Given a rigid body under general motion, a set of its 
points located on a line C undergoes the identical minimum-magnitude 
velocity Vq parallel to the angular velocity. 

Definition 3.4.1 The line containing the points of a rigid body undergoing 
minimum-magnitude velocities is called the instant screw axis (ISA) of the 
body under the given motion. 



3.4.1 The Instant Screw of a Rigid-Body Motion 

From Theorem 3.4.2, the instantaneous motion of a body is equivalent to 
that of the bolt of a screw of axis C , the ISA. Clearly, as the body moves, 
the ISA changes, and the motion of the body is called an instantaneous 
screw. Moreover, since Vq is parallel to it can be written in the form 

v„ = (3,54) 



where vq is a scalar quantity denoting the signed magnitude of Vq and bears 
the sign of Vq • u>. Furthermore, the pitch of the instantaneous screw, p' , is 
defined as 



P 



/ 



^0 

ll^ll 



p • UJ 

hF 



or p' 



2ttvq 



(3.55) 



which thus bears units of m/rad or correspondingly, of m/turn. 

Again, the ISA L' can be specified uniquely through its Pliicker coordi- 
nates, stored in the p£/ array defined as 



VC 




(3.56) 



where e' and n' are, respectively, the unit vector defining the direction of 
L' and its moment about the origin, i.e., 

e'=-T^, n' = pxe' (3.57) 

p being the position vector of any point of the ISA. Clearly, e' is defined 
uniquely but becomes trivial when the rigid body instantaneously under- 
goes a pure translation, i.e., a motion during which, instantaneously, tu = 0. 
In this case, e' is defined as the unit vector parallel to the associated dis- 
placement field. Thus, an instantaneous rigid-body motion is defined by a 
line a pitch p' , and an amplitude ||w||. Such a motion is, then, fully 
determined by six independent parameters, namely, the four independent 
Pliicker coordinates of its pitch, and its amplitude. A line supplied with 
a pitch is, in general, called a screw; a screw supplied with an amplitude 
representing the magnitude of an angular velocity provides the represen- 
tation of an instantaneous rigid-body motion that is sometimes called the 
twi.st, an item that will be discussed more in detail below. 
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Hence, the instantaneous screw is fully defined by six independent real 
numbers. Moreover, such as in the case of the screw motion, the pitch of 
the instantaneous screw can attain values from — oo to +oo. 

The ISA can be alternatively described in terms of the position vector pg 
of its point lying closest to the origin. Expressions for pg in terms of the po- 
sition and the velocity of an arbitrary body-point and the angular velocity 
are derived below. To this end, we decompose p into two orthogonal com- 
ponents, p|| and py, along and transverse to the angular- velocity vector, 
respectively. To this end, a is first decomposed into two such orthogonal 
components, a|| and ay, the former being parallel, the latter normal to the 
ISA, i.e., 

a = a|| + ay (3.58) 

These orthogonal components are given as 



a = a • u}- 









ay = 1 - 









(3.59) 



In the derivation of eq.(3.59) we have used the identity introduced in 
eq.(2.39), namely, 

— \\u>\\^l (3.60) 

Upon substitution of eq.(3.59) into eq.(3.52), we obtain 



P 




ri^a + f2(p — a) 



(3.61) 



Of the three components of p, the first, henceforth referred to as its axial 
component, is parallel, the last two being normal to u>. The sum of the last 
two components is referred to as the normal component of 'p. From eq. (3.61) 
it is apparent that the axial component is independent of p, while the 
normal component is a linear function of p. An obvious question now arises: 
For an arbitrary motion, is it possible to find a certain point of position 
vector p whose velocity normal component vanishes? The vanishing of the 
normal component obviously implies the minimization of the magnitude of 
p. The condition under which this happens can now be written as 



py = 0 



n(p - a) - \ a = 0 (3.62) 

which can be further expressed as a vector equation linear in p, namely. 




rip = ri a + 



2 



(3.63) 
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or 



ri(p — r) = 0 

with r defined as 

= ^+ 

II^P 

and hence, a possible solution of the foregoing problem is 

p = r = a + --^r2a 



(3.64a) 

(3.64b) 



(3.65) 



However, this solution is not unique, for eq. (3.64a) does not require that 
p— r be zero, only that this difference he in the nullspace of i.e., that p— r 

be linearly dependent with u). In other words, if a vector o.(jJ is added to p, 
then the sum also satisfies eq.(3.63). It is then apparent that eq.(3.63) does 
not determine a single point whose normal velocity component vanishes 
but a set of points lying on the ISA, and thus, other solutions are possible. 
For example, we can find the point of the ISA lying closest to the origin. To 
this end, let pg be the position vector of that point. This vector is obviously 
perpendicular to tu, i.e., 

tu^Pg = 0 (3.66) 

Next, eq.(3.63) is rewritten for pg, and eq.(3.66) is adjoined to it, thereby 
deriving an expanded linear system of equations, namely. 



Apg = b 



(3.67) 



where A is a 4 x 3 matrix and b is a 4-dimensional vector, both of which 
are given below: 



A = 



n 


y. — 


'r2a+ (l/||w||2)r22d' 


T 


1 ^ — 


0 



(3.68) 



This system is of the same nature as that appearing in eq.(3.11), and hence, 
it can be solved for pg following the same procedure. Thus, both sides of 
eq.(3.67) are multiplied from the left by A^, thereby obtaining 

A^Ap'o = A^b (3.69) 



where 

A^A = 0^0 + (3.70) 

Moreover, from eq.(3.60), the rightmost side of the foregoing relation be- 
comes ||tu|pl, and hence, the matrix coefficient of the left-hand side of 
eq.(3.69) and the right-hand side of the same equation reduce, respectively, 
to 



A^A=||wfl, A^b = f2(a- f2a) 



(3.71) 
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Upon substitution of eq.(3.71) into eq.(3.69) and further solving for pg, the 
desired expression is derived: 



ri(a — ria) _ tu X (a — tu X a) 

II^P " HP 



(3.72) 



Thus, the instantaneous screw is fully defined by an alternative set of six 
independent scalars, namely, the three components of its angular velocity 
uj and the three components of the velocity of an arbitrary body point A, 
denoted by a. As in the case of the screw motion, we can also represent the 
instantaneous screw by a line and two additional parameters, as we explain 
below. 



S-4-S The Twist of a Rigid Body 

A line, as we saw earlier, is fully defined by its 6-dimensional Pliicker array, 
which contains only four independent components. Now, if a pitch p is 
added as a fifth feature to the line or correspondingly, to its Pliicker array, 
we obtain a screw s, namely. 



e 

p X e + pe 



(3.73) 



An amplitude is any scalar A multiplying the foregoing screw. The am- 
plitude produces a twist or a wrench, to be discussed presently, depending 
on its units. The twist or the wrench thus defined can be regarded as an 
eight-parameter array. These eight parameters, of which only six are in- 
dependent, are the amplitude, the pitch, and the six Pliicker coordinates 
of the associated line. Clearly, a twist or a wrench is defined completely 
by six independent real numbers. More generally, a twist can be regarded 
as a 6-dimensional array defining completely the velocity field of a rigid 
body, and it comprises the three components of the angular velocity and 
the three components of the velocity of any of the points of the body. 

Below we elaborate on the foregoing concepts. Upon multiplication of 
the screw appearing in eq.(3.73) by the amplitude A representing the mag- 
nitude of an angular velocity, we obtain a twist t, namely. 



|_p X (Ae) +p(Ae) J 

where the product Ae can be readily identified as the angular velocity uj 
parallel to vector e, of magnitude A. Moreover, the lower part of t can be 
readily identified with the velocity of a point of a rigid body. Indeed, if we 
regard the line C and point O as sets of points of a rigid body B moving 
with an angular velocity u> and such that point P moves with a velocity 
puj parallel to the angular velocity, then the lower vector of t, denoted by 
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V, represents the velocity of point O, i.e., 

V = —(jJ X p +puj 



We can thus express the twist t as 

t ^ ^ (3.74) 

A special case of great interest in kinematics is the screw of infinitely 
large pitch. The form of this screw is derived, very informally, by taking 
the limit of expression (3.73) as p ^ oo, namely, 

lim ® = lim ( pj , 

p^oo[pxe+peJ p^oo Y [(pxe)/p + e 

which readily leads to 

lim ^ = ( lim p] 

p^oo p X © -\- pB yp — J © 

The screw of infinite pitch s^o is defined as the 6-dimensional array appear- 
ing in the above equation, namely, 

(3.75) 

Note that this screw array is identical to the Pliicker array of the line at 
infinity lying in a plane of unit normal e. 

The twist array, as defined in eq.(3.74), with u> on top, represents the 
ray coordinates of the twist. An exchange of the order of the two Cartesian 
vectors of this array, in turn, gives rise to the axis coordinates of the twist. 

The foregoing twist was also termed motor by Everett (1875). As Phillips 
(1990) points out, the word motor is an abbreviation of moment and vector. 
An extensive introduction into motor algebra was published by von Mises 
(1924), a work that is now available in English (von Mises, 1996). Roth 
(1984), in turn, provided a summary of these concepts, as applicable to 
robotics. The foregoing array goes also by other names, such as the German 
Kinemate. 

The relationships between the angular-velocity vector and the time de- 
rivatives of the invariants of the associated rotation are linear. Indeed, let 
the three sets of four invariants of rotation, namely, the natural invariants, 
the linear invariants, and the Euler-Rodrigues parameters be grouped in 
the 4-dimensional arrays u, A, and 77, respectively, i.e.. 







90 



3. Fundamentals of Rigid-Body Mechanics 



We then have the linear relations derived in full detail elsewhere (Angeles, 
1988), and outlined in Appendix A for quick reference, namely, 

z> = Ntu, A = Ltu, Tj = Huj (3.77a) 

with N, L, and H defined as 

^ r [sin<))/(2(l - cos())))](l - ee^) - (1/2)E 

^ (l/2)[tr(Q)l-Q]- 
— (sin<)))e^ ’ 

„ _ 1 cos(())/2)l — sin(())/2)E 
~ 2 — sm((j)/2)e^ 

where, it is recalled, tr(-) denotes the trace of its square matrix argument 
(•), i.e., the sum of the diagonal entries of that matrix. 

The inverse relations of those shown in eqs.(3.77a) are to be derived by 
resorting to the approach introduced when solving eq.(3.67) for pg, thereby 
obtaining 

u! = Nz> = LA = H ?7 (3.78a) 

the 3x4 matrices N, L, and H being defined below: 

N = [ (sin ()))1 + (1 — cos ()))E e], (3.78b) 

L = [ 1 + [(sin<)))/(l + cos()))]E — [(sin<)))/(l + cos<)))]e] , (3.78c) 

H = 2 [ [cos(())/2)]l + [sin(())/2)]E — [sin(<))/2)]e] (3.78d) 

As a consequence, we have the following: 

Caveat The angular velocity vector is not a time- derivative, i.e., no Carte- 
sian vector exists whose time- derivative is the angular-velocity vector. 

However, matrices N, L, and H of eqs.(3.77b-d) can be regarded as 
integration factors that yield time-derivatives. 

Now we can write the relationship between the twist and the time-rate 
of change of the 7-dimensional pose array s, namely, 

s = Tt (3.79) 



(3.77b) 

(3.77c) 

(3.77d) 



where 





(3.80) 



in which O and O 43 are the 3x3 and the 4x3 zero matrices, while 
1 is the 3x3 identity matrix and F is, correspondingly, N, L, or H, 
depending upon the invariant representation chosen for the rotation. The 
inverse relationship of eq.(3.79) takes the form 



t = Ss 



(3.81a) 
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where 




(3.81b) 



in which O 34 is the 3x4 zero matrix. Moreover, F is one of N, L, or 
H, depending on the rotation representation adopted, namely, the nat- 
ural invariants, the linear invariants, or the Euler-Rodrigues parameters, 
respectively. 

A formula that relates the twist of the same rigid body at two different 
points is now derived. Let A and P be two arbitrary points of a rigid body. 
The twist at each of these points is defined as 



tA 



U! 



VA 



tp 



U! 

Vp 



Moreover, eq.(3.52) can be rewritten as 

Vp = va + (a — p) X tu 
Combining eq.(3.82) with eq.(3.83) yields 

tp = Ut A 



(3.82) 



(3.83) 



(3.84) 



where 



U = 



1 o 

A-P 1 



(3.85) 



with the 6x6 matrix U defined as in eq.(3.31), while A and P denote the 
cross-product matrices of vectors a and p, respectively. Thus, eq.(3.84) can 
be fairly called the twist-transfer formula. 



3.5 Acceleration Analysis of Rigid-Body Motions 

Upon differentiation of both sides of eq.(3.51) with respect to time, one 
obtains 

p = a + f2(p — a) + ri(p — a) (3.86) 

Now, eq.(3.51) is solved for p — a, and the expression thus resulting is 
substituted into eq.(3.86), thereby obtaining 

p = a + (d + f2^)(p — a) (3.87) 

where the matrix sum in parentheses is termed the angular- acceleration 
matrix of the rigid-body motion and is represented by W, i.e., 

W = d + 



(3.88) 
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Clearly, the first term of the right-hand side of eq.(3.88) is skew-symmetric, 
whereas the second one is symmetric. Thus, 

vect(W) = vect(ii) = u) (3.89) 

u) being termed the angular- acceleration vector of the rigid-body motion. 
We have now an interesting result, namely, 

tr(W) = tr(f22) = tr(-||wf 1 + 

= — ||tu||^tr(l) + u) ■ u) = — 2||tu||^ (3.90) 

Moreover, eq.(3.87) can be written as 

p = a + th X (p — a) + tu X [tu X (p — a)] (3.91) 

On the other hand, the time derivative of t, henceforth referred to as the 
twist rate, is displayed below: 



t = 



6j 

V 



(3.92) 



in which v is the acceleration of a point of the body. The relationship 
between the twist rate and the second time derivative of the screw is derived 
by differentiation of both sides of eq.(3.79), which yields 



s = Tt + Tt 



(3.93) 



where 



rp _ F O43 

o o 



(3.94) 



and F is one of N, L, or H, accordingly. The inverse relationship of eq.(3.93) 
is derived by differentiating both sides of eq. (3.81a) with respect to time, 
which yields 



t = Ss + Ss 



(3.95) 



where 



F O 

O34 O 



(3.96) 



with O and O34 already defined in eq. (3.81b) as the 3x3 and the 3x4 

zero matrices, respectively, while F is one of N, L, or H, according with 
the type of rotation representation at hand. 

Before we take to differentiating the foregoing matrices, we introduce a 
few definitions: Let 

r 




(3.97a) 
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i.e., 



u = sin (f>e, uq = cos </>, 



r = sin 






2 



ro = cos 



2 



(3.97b) 



Thus, the time derivatives sought take on the forms 



N 



1 

4(1 — cos (p) 



B 

e 



L 



(l/2)[ltr(Q)-Q] 
(l/2)cu^[ltr(Q) - Q^] 

-(w • u)l - (l/2)r2Q 
(l/2)cu^[ltr(Q) - Q^] 



H 



rol - R 



(3.98a) 



(3.98b) 



(3.98c) 



where we have used the identities below, which are derived in Appendix A. 

tr(Q) = tr(f2Q) = — 2tu^u (3.98d) 



Furthermore, R denotes the cross-product matrix of r, and B is defined as 
B = — 2(e • tu)l + 2(3 — cos 4>){e ■ uj)ee^ — 2(1 + sin <f>)uje^ 



— (2 cos (f) + sin (j))eu)^ — (sin — (e • tu)E] (3.98e) 

Moreover, 

N = [ <))(cos ()))1 + <))(sin 0)E e] (3.99a) 

L=[Y/D li] (3.99b) 

H=[rol + R — r] (3.99c) 

where V and D are defined below: 

V = U - (uu^ + uu^) - ^(U -uu^) (3.99d) 

D = I + uq (3.99e) 



with U denoting the cross-product matrix of u. 



3.6 Rigid-Body Motion Referred to Moving 
Coordinate Axes 

Although in kinematics no “preferred” coordinate system exists, in dynam- 
ics the governing equations of rigid-body motions are valid only in inertial 
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frames. An inertial frame can be defined as a coordinate system that trans- 
lates with uniform velocity and constant orientation with respect to the 
stars. Thus, it is important to refer vectors and matrices to inertial frames, 
but sometimes it is not possible to do so directly. For instance, a spacecraft 
can be supplied with instruments to measure the velocity and the acceler- 
ation of a satellite drifting in space, but the measurements taken from the 
spacecraft will be referred to a coordinate frame fixed to it, which is not in- 
ertial. If the motion of the spacecraft with respect to an inertial coordinate 
frame is recorded, e.g., from an Earth-based station, then the acceleration 
of the satellite with respect to an inertial frame can be computed using the 
foregoing information. How to do this is the subject of this section. 

In the realm of kinematics, it is not necessary to distinguish between 
inertial and noninertial coordinate frames, and hence, it will suffice to call 
the coordinate systems involved fixed and moving. Thus, consider the fixed 
coordinate frame X, Y, Z, which will be labeled X, and the moving coor- 
dinate frame T, y, and Z, which will be labeled A4, both being depicted 
in Fig. 3.4. Moreover, let Q be the rotation matrix taking frame Z into 
the orientation of A4, and o the position vector of the origin of A4 from 
the origin of T . Further, let p be the position vector of point P from the 
origin of Z and p the position vector of the same point from the origin of 
A4. From Fig. 3.4 one has 

[v]t =[o]t+[p]t (3.100) 

where it will be assumed that p is not available in frame Z, but in A4. 
Hence, 

[p]t = [CI]t[p]m (3.101) 

Substitution of eq.(3.101) into eq.(3.100) yields 

[v]t =[o]t +[Q.]t[p\m (3.102) 

Now, in order to compute the velocity of P, both sides of eq.(3.102) are 
differentiated with respect to time, which leads to 

[p]j^ = [o]t + [Q]Ap]m + [Q]Ap]m (3.103) 

Furthermore, from the definition of eq.(3.46), we have 

[Q]jc- = (3.104) 

Upon substitution of the foregoing relation into eq.(3.103), we obtain 

[p]t = [o]t + [^]AQ]Ap]m + [Q]Ap]m (3.105) 

which is an expression for the velocity of P in X in terms of the velocity of 
P in A4 and the twist of A4 with respect to Z . Next, the acceleration of 
P in P is derived by differentiation of both sides of eq.(3.105) with respect 
to time, which yields 

[ p ] j ^ = [o]t + [^]t[Q,]t[p]m + [^]t[Q]t[p]m 

+ [^]t[Q]t[p]m + [Q]t[p]m + [Q]t[p]m (3.106) 




3.7 Static Analysis of Rigid Bodies 95 




FIGURE 3.4. Fixed and moving coordinate frames. 

Further, upon substitution of identity (3.104) into eq.(3.106), we obtain 

[p]u = [o]t + + [^‘^]t)[Q]Ap]m 

+‘2[^]t[Q,]t[p]m + [Q,]t[p]m (3.107) 

Moreover, from the results of Section 3.5, it is clear that the first two 
terms of the right-hand side of eq.(3.107) represent the acceleration of P as 
a point of A4, whereas the fourth term is the acceleration of P measured 
from A4. The third term is what is called the Coriolis acceleration, as it was 
first pointed out by the French mathematician Gustave Gaspard Coriolis 
(1835). 

3.7 Static Analysis of Rigid Bodies 

Germane to the velocity analysis of rigid bodies is their force-and-moment 
analysis. In fact, striking similarities exist between the velocity relations 
associated with rigid bodies and the forces and moments acting on them. 
From elementary statics it is known that the resultant of all external ac- 
tions, i.e., forces and moments, exerted on a rigid body can be reduced to 
a force f acting at a point, say A, and a moment n^- Alternatively, the 
aforementioned force f can be defined as acting at an arbitrary point P 
of the body, as depicted in Fig. 3.5, but then the resultant moment, rip, 
changes correspondingly. 

In order to establish a relationship between ua and rip, the moment of 
the first system of force and moment with respect to point P is equated to 




96 



3. Fundamentals of Rigid-Body Mechanics 




FIGURE 3.5. Equivalent systems of force and moment acting on a rigid body. 



the moment about the same point of the second system, thus obtaining 

rip = ua + (a — p) X f (3.108) 

which can be rewritten as 

rip = riA + f X (p — a) (3.109) 

whence the analogy with eq.(3.52) is apparent. Indeed, rip and ua of 
eq.(3.109) play the role of the velocities of P and A, p and a, respec- 
tively, whereas f of eq.(3.109) plays the role of tu of eq.(3.52). Thus, similar 
to Theorem 3.4.2, one has 

Theorem 3.7.1 For a given system of forces and moments acting on a 
rigid body, if the resultant force is applied at any point of a particular line 
C" , then the resultant moment is of minimum magnitude. Moreover, that 
minimum-magnitude moment is parallel to the resultant force. 



Hence, the resultant of the system of forces and moments is equivalent to 
a force f acting at a point of £" and a moment n, with both f and n parallel 
to £". Paraphrasing the definition of the ISA, one defines line £" as the 
axis of the wrench acting on the body. Let iiq be the minimum-magnitude 
moment. Clearly, iiq can be expressed as Vq was in eq.(3.54), namely, as 



f rip • f 

no = no^, = ^ 



Moreover, the pitch of the wrench, p" , is defined as 



_ n-o rip • f „ 27 rrip • f 



(3.111) 
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which again has units of m/rad or correspondingly, of m/turn. Of course, 
the wrench axis can be defined by its Pliicker array, P£", i.e.. 



P£ 



n 



n 



p X e 



(3.112) 



where e" is the unit vector parallel to £", n" is the moment of £" about 
the origin, and p is the position vector of any point on £". 

The wrench axis is fully specified, then, by the direction of f and point 
Pg' lying closest to the origin of position vector pg, which can be derived 
by analogy with eq.(3.72), namely, as 

Po = f (S.IU) 

Similar to Theorem 3.4.1, one has 



Theorem 3.7.2 The projection of the resultant moment of a system of 
moments and forces acting on a rigid body that arises when the resultant 
force is applied at an arbitrary point of the body onto the wrench axis is 
constant. 



Prom the foregoing discussion, then, the wrench applied to a rigid body 
can be fully specified by the resultant force f acting at an arbitrary point P 
and the associated moment, rip. We shall derive presently the counterpart 
of the 6-dimensional array of the twist, namely, the wrench array. Upon 
multiplication of the screw of eq.(3.73) by an amplitude A with units of 
force, what we will obtain would be a wrench w, i.e., a 6-dimensional array 
with its first three components having units of force and its last components 
units of moment. We would like to be able to obtain the power developed 
by the wrench on the body moving with the twist t by a simple inner 
product of the two arrays. However, because of the form the wrench w 
has taken, the inner product of these two arrays would be meaningless, 
for it would involve the sum of two scalar quantities with different units, 
and moreover, each of the two quantities is without an immediate physical 
meaning. In fact, the first scalar would have units of force by frequency 
(angular velocity by force), while the second would have units of moment 
of moment multiplied by frequency (velocity by moment), thereby leading 
to a physically meaningless result. This inconsistency can be resolved if we 
redefine the wrench not simply as the product of a screw by an amplitude, 
but as a linear transformation of that screw involving the 6x6 array F 
defined as 



r = 



0 1 

1 o 



(3.114) 



where O and 1 denote, respectively, the 3x3 zero and identity matrices. 
Now we define the wrench as a linear transformation of the screw s defined 
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in eq.(3.73). This transformation is obtained upon multiplying s by the 
product AT, the amplitude A having units of force, i.e., 



w = Mrs = 



p X (Me) +p(Me) 
Me 



The foregoing wrench is said to be given in axis coordinates, as opposed to 
the twist, which was given in ray coordinates. 

Now, the first three components of the foregoing array can be readily 
identified as the moment of a force of magnitude M acting along a line of 
action given by the Pliicker array of eq.(3.fl2), with respect to a point P, 
to which a moment parallel to that line and of magnitude pA is added. 
Moreover, the last three components of that array pertain apparently to 
a force of magnitude M and parallel to the same line. We denote here the 
above-mentioned moment by n and the force by f, i.e.. 



f = Me, n = p X f + pf 



The wrench w is then defined as 



w = 



n 

f 



(3.115) 



which can thus be interpreted as a representation of a system of forces and 
moments acting on a rigid body, with the force acting at point P of the 
body B defined above and a moment n. Under these circumstances, we say 
that w acts at point P of B. 

With the foregoing definitions it is now apparent that the wrench has 
been defined so that the inner product t^w will produce the power II 
developed by w acting at P when B moves with a twist t defined at the 
same point, i.e., 

n = t^w (3.116) 

When a wrench w that acts on a rigid body moving with the twist t 
develops zero power onto the body, we say that the wrench and the twist 
are reciprocal to each other. By the same token, the screws associated with 
that wrench-twist pair are said to be reciprocal. More specifically, let the 
wrench and the twist be given in terms of their respective screws, Syj and 
St, as 

w = WTsy„, t = Tst, (3.117) 

where W and T are the amplitudes of the wrench and the twist, respectively, 
while r is as defined in eq.(3.114). Thus, the two screws s^^ and Sj are 
reciprocal if 

(rs^)^st = s^r^st = o (3.118) 

and by virtue of the symmetry of F, the foregoing relation can be further 
expressed as 



s^Fst = 0 or sJTsiu 



0 



(3.119) 
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Now, if A and P are arbitrary points of a rigid body, we define the wrench 
at these points as 



WA = 


Ha 

f 


, Wp = 


np 

f 


(3.120) 


Therefore, eq.(3.108) leads to 












wp = Vwa 




(3.121) 


where 














1 A - P 




(3.122) 


V 


= 


0 1 





and A and P were defined in eq.(3.85) as the cross-product matrices of 
vectors a and p, respectively. Thus, wp is a linear transformation of w^. 
By analogy with the twist-transfer formula of eq.(3.84), eq.(3.12f) is termed 
here the wrench-transfer formula. 

Multiplying the transpose of eq.(3.84) by eq.(3.12f) yields 



tpWp = t^j^U^VwA 



(3.123) 



where 



U^V 



1 -A + P 


1 A-P 


0 1 


0 1 



l6x6 



(3.124) 



with lexe denoting the 6x6 identity matrix. Thus, tpWp = t^(wA, as 
expected, since the wrench develops the same amount of power, regardless 
of where the force is assumed to be applied. Also note that an interesting 
relation between U and V follows from eq.(3.124), namely. 



v-i = 



(3.125) 
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The equations governing the motion of rigid bodies are recalled in this 
section and cast into a form suitable to multibody d}mamics. To this end, 
a few definitions are introduced. If a rigid body has a mass density p, which 
need not be constant, then its mass m is defined as 

m = pdB (3.126) 

Jb 

where B denotes the region of the 3-dimensional space occupied by the 
body. Now, if p denotes the position vector of an arbitrary point of the 
body, from a previously defined origin O, the mass first moment of the 
body with respect to O, qo, is defined as 



qo = 




(3.127) 
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Furthermore, the mass second moment of the body with respect to O is 
defined as 

lo = / p[{p ■P)i -Pp'^]dB (3.128) 

JB 

which is clearly a symmetric matrix. This matrix is also called the moment- 
of-inertia matrix of the body under study with respect to O. One can readily 
prove the following result: 

Theorem 3.8.1 The moment of inertia of a rigid body with respect to a 
point O is positive definite. 

Proof : All we need to prove is that for any vector tu, the quadratic form 
is positive. But this is so, because 



/ p[\\pf\\ujf-{p-ujf\dB (3.129) 

JB 

Now, we recall that 

p • tu = IIpII ||tu|| cos(p, tu) (3.130) 

in which (p, u>) stands for the angle between the two vectors within the 
parentheses. Substitution of eq.(3.130) into eq.(3.129) leads to 



/ p||p|n|w|p[l -cos^(p,w)]dB 
JB 

= f p\\pf\\<^fsm'^{P,<^)dB 

JB 

which is a positive quantity that vanishes only in the ideal case of a slender 
body having all its mass concentrated along a line passing through O and 
parallel to tu, which would thus render sin(p, tu) = 0 within the body, 
thereby completing the proof. 

Alternatively, one can prove the positive definiteness of the mass moment 
of inertia based on physical arguments. Indeed, if vector u> of the previous 
discussion is the angular velocity of the rigid body, then the quadratic form 
of eq.(3.129) turns out to be twice the kinetic energy of the body. Indeed, 
the said kinetic energy, denoted by T, is defined as 

l^^pWpfdB 

where p is the velocity of any point P of the body. For the purposes of this 
discussion, it will be assumed that point O, about which the second moment 
is defined, is a point of the body that is instantaneously at rest. Thus, if 
this point is defined as the origin of the Euclidean space, the velocity of 
any point of the body, moving with an angular velocity tu, is given by 



p = tu X p 
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which can be rewritten as 

p = -Pw 

with P defined as the cross-product matrix of p. Hence, 

||pf = (Pw)^Pw = w^P^Pw = -w^P^w 

Moreover, by virtue of eq.(2.39), the foregoing expression is readily re- 
ducible to 

llpf =cu^(||p||2l-pp^)cu (3.131) 

Therefore, the kinetic energy reduces to 

-PP^)<^dB (3.132) 

^ Jb 

and since the angular velocity is constant throughout the body, it can be 
taken out of the integral sign, i.e.. 



T 




I ddipfl 

B 



pp^)dB 



(jj 



(3.133) 



The term inside the brackets of the latter equation is readily identified as 
lo, and hence, the kinetic energy can be written as 



T 



1 

2 






(3.134) 



Now, since the kinetic energy is a positive-definite quantity, the quadratic 
form of eq.(3.134) is consequently positive-definite as well, thereby proving 
the positive-definiteness of the second moment. 

The mass center of a rigid body, measured from O, is defined as a point 
C, not necessarily within the body — think of a homogeneous torus — of 
position vector c given by 



c = 



go 

m 



(3.135) 



Naturally, the mass moment of inertia of the body with respect to its 
centroid is defined as 



Ic= p[ ||r|pl - (3.136) 

Jb 

where r is defined, in turn, as 



r = p — c (3.137) 

Obviously, the mass moment of inertia of a rigid body about its mass cen- 
ter, also termed its centroidal mass moment of inertia, is positive-definite 
as well. In fact, the mass — or the volume, for that matter — moment of 
inertia of a rigid body with respect to any point is positive-definite. As a 
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consequence, its three eigenvalues are positive and are referred to as the 
principal moments of inertia of the body. The eigenvectors of the inertia 
matrix are furthermore mutually orthogonal and define the principal axes 
of inertia of the body. These axes are parallel to the eigenvectors of that 
matrix and pass through the point about which the moment of inertia is 
taken. Note, however, that the principal moments and the principal axes 
of inertia of a rigid body depend on the point with respect to which the 
moment of inertia is defined. Moreover, let Iq and Ic be defined as in 
eqs.(3.128) and (3.136), with r defined as in eq.(3.137). It is possible to 
show that 

lo = Ic + m(||c|pl - cc^) (3.138) 

Furthermore, the smallest principal moment of inertia of a rigid body at- 
tains its minimum value at the mass center of the body. The relationship 
appearing in eq.(3.138) constitutes the Theorem of Parallel Axes. 

Henceforth, we assume that c is the position vector of the mass center in 
an inertial frame. Now, we recall the Newton-Euler equations governing the 
motion of a rigid body. Let the body at hand be acted upon by a wrench of 
force f applied at its mass center, and a moment nc. The Newton equation 
then takes the form 

f = me (3.139a) 

whereas the Euler equation is 

nc = Ic<^ + tu X leix) (3.139b) 

The momentum m and the angular momentum he of a rigid body moving 
with an angular velocity u> are defined below, the angular momentum being 
defined with respect to the mass center. These are 

m = me, he = Ic<^ (3.140) 

Moreover, the time-derivatives of the foregoing quantities are readily com- 
puted as 

hi = me, he = Ic<^ + X Ic<^ (3.141) 

and hence, eqs.(3.139a & b) take on the forms 

f = hi, nc = he (3.142) 

The set of equations (3.139a) and (3.139b) are known as the Newton-Euler 
equations. These can be written in a more compact form as we describe 
below. First, we introduce a 6 x 6 matrix M that following von Mises 
(1924), we term the inertia dyad, namely. 



M = 



Ic O 
O ml 



(3.143) 
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where O and 1 denote the 3x3 zero and identity matrices. A similar 
6x6 matrix was defined by von Mises under the above name. However, 
von Mises’s inertia dyad is full, while the matrix defined above is block- 
diagonal. Both matrices, nevertheless, denote the same physical property 
of a rigid body, i.e., its mass and moment of inertia. Now the Newton-Euler 
equations can be written as 



Mt + WMt = w 



(3.144) 



in which matrix W, which we shall term, by similarity with the inertia 
dyad, the angular-velocity dyad, is defined, in turn, as 



W = 



n o 
o o 



(3.145) 



with already defined as the angular- velocity matrix; it is, of course, the 
cross-product matrix of the angular- velocity vector u). Note that the twist 
of a rigid body lies in the nullspace of its angular- velocity dyad, i.e.. 



Wt = 0 



(3.146) 



Further definitions are introduced below: The momentum screw of the 
rigid body about the mass center is the 6-dimensional vector /j, defined as 



Icoj 

me 



Mt 



(3.147) 



Furthermore, from eqs.(3.141) and definition (3.147), the time-derivative 
of fj, can be readily derived as 

/Lt = Mt + W/LX = Mt + WMt (3.148) 

The kinetic energy of a rigid body undergoing a motion in which its mass 
center moves with velocity c and rotates with an angular velocity uj is given 

by 

T = im||c|p + (3.149) 

From the foregoing definitions, then, the kinetic energy can be written in 
compact form as 

T = it^Mt (3.150) 

Finally, the Newton-Euler equations can be written in an even more com- 

pact form as 

fj, = w (3.151) 

which is a 6-dimensional vector equation. 




4 



Kinetostatics of Simple Robotic 
Manipulators 



4.1 Introduction 

This chapter is devoted to the kinetostatics of robotic manipulators of the 
serial type, i.e., to the kinematics and statics of these systems. The study is 
general, but with regard to what is called the inverse kinematics problem, 
we limit the chapter to decoupled manipulators, to be defined below. The 
inverse displacement analysis of general six-axis manipulators is addressed 
in Chapter 8. 

More specifically, we will define a serial, n-axis manipulator. In connec- 
tion with this manipulator, additionally, we will (*) introduce the Denavit- 
Hartenberg notation for the definition of link frames that uniquely deter- 
mine the architecture and the configuration, or posture, of the manipulator 
at hand; (m) define the Cartesian and joint coordinates of this manipulator; 
and {iii) introduce its Jacobian matrix. 

Moreover, with regard to six-axis manipulators, we will (*) define decou- 
pled manipulators and provide a procedure for the solution of their displace- 
ment inverse kinematics; (n) formulate and solve the velocity-resolution 
problem, give simplified solutions for decoupled manipulators, and iden- 
tify their singularities] (Hi) define the workspace of a three-axis positioning 
manipulator and provide means to display it; (iv) formulate and solve the 
acceleration-resolution problem and give simplified solutions for decoupled 
manipulators; and (u) analyze manipulators statically, while giving simpli- 
fied analyses for decoupled manipulators. While doing this, we will devote 
special attention to planar manipulators. 
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4.2 The Denavit-Hartenberg Notation 

One of the first tasks of a robotics engineer is the kinematic modeling of 
a robotic manipulator. This task consists of devising a model that can be 
unambiguously (i) described to a control unit through a database and (n) 
interpreted by other robotics engineers. The purpose of this task is to give 
manipulating instructions to a robot, regardless of the dynamics of the 
manipulated load and the robot itself. The simplest way of kinematically 
modeling a robotic manipulator is by means of the concept of kinematic 
chain. A kinematic chain is a set of rigid bodies, also called links, coupled by 
kinematic pairs. A kinematic pair is, then, the coupling of two rigid bodies 
so as to constrain their relative motion. We distinguish two basic types of 
kinematic pairs, namely, upper and lower kinematic pairs. An upper kine- 
matic pair arises between rigid bodies when contact takes place along a 
line or at a point. This type of coupling occurs in cam- and- follower mecha- 
nisms, gear trains, and roller bearings, for example. A lower kinematic pair 
occurs when contact takes place along a surface common to the two bodies. 
Six different types of lower kinematic pairs can be distinguished (Harten- 
berg and Denavit, 4964; Angeles, 4982), but all these can be produced from 
two basic types, namely, the rotating pair, denoted by R and also called 
revolute, and the sliding pair, represented by P and also called prismatic. 

The common surface along which contact takes place in a revolute pair is 
a circular cylinder, a t}rpical example of this pair being the coupling through 
journal bearings. Thus, two rigid bodies coupled by a revolute can rotate 
relative to each other about the axis of the common cylinder, which is thus 
referred to as the axis of the revolute, but are prevented from undergoing 
relative translations as well as rotations about axes other than the cylinder 
axis. On the other hand, the common surface of contact between two rigid 
bodies coupled by a prismatic pair is a prism of arbitrary cross section, and 
hence, the two bodies coupled in this way are prevented from undergoing 
any relative rotation and can move only in a pure-translation motion along 
a direction parallel to the axis of the prism. As an example of this kinematic 
pair, one can cite the dovetail coupling. Note that whereas the revolute axis 
is a totally defined line in three-dimensional space, the prismatic pair has 
no defined axis; this pair has only a direction. That is, the prismatic pair 
does not have a particular location in space. Bodies coupled by a revolute 
and a prismatic pair are shown in Fig. 4.f. 

Serial manipulators will be considered in this chapter, their associated 
kinematic chains thus being of the simple type, i.e., each and every link 
is coupled to at most two other links. A simple kinematic chain can be 
either closed or open. It is closed if each and every link is coupled to two 
other links, the chain then being called a linkage] it is open if it contains 
exactly two links, the end ones, that are coupled to only one other link. 
Thus, simple kinematic chains studied in this chapter are open, and in the 
particular robotics terminology, their first link is called the manipulator- 
base, whereas their last link is termed the end-effector (EE). 

Thus, the kinematic chains associated with manipulators of the serial 
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FIGURE 4.1. Revolute and prismatic pair. 

type are composed of binary links, the intermediate ones, and exactly two 
simple links, those at the ends. Hence, except for the end links, all links 
carry two kinematic pairs, and as a consequence, two pair axes — however, 
notice that a prismatic pair has a direction but no axis. In order to uniquely 
describe the architecture of a kinematic chain, i.e., the relative location and 
orientation of its neighboring pair axes, the Denavit-Hartenberg nomen- 
clature (Denavit and Hartenberg, 1955) is introduced. To this end, links 
are numbered 0, 1, . . . , n, the *th pair being defined as that coupling the 
(* — l)st link with the *th link. Hence, the manipulator is assumed to be 
composed of n + 1 links and n pairs; each of the latter can be either R or P, 
where link 0 is the fixed base, while link n is the end-effector. Next, a coor- 
dinate frame Pi is defined with origin Oi and axes Xi, Yi, Zi. This frame 
is attached to the (* — l)st link — not to the *th link! — for * = 1, ...,n+l. 
For the first n frames, this is done following the rules given below: 

1. Zi is the axis of the *th pair. Notice that there are two possibilities of 
defining the positive direction of this axis, since each pair axis is only 
a line, not a directed segment. Moreover, the Zi axis of a prismatic 




FIGURE 4.2. Definition of Xi when Zi-i and Zr. (a) are skew; (b) intersect; and 
(c) are parallel. 
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pair can be located arbitrarily, since only its direction is defined by 
the axis of this pair. 

2. Xi is defined as the common perpendicular to and Zj, directed 
from the former to the latter, as shown in Fig. 4.2a. Notice that if 
these two axes intersect, the positive direction of Xi is undefined and 
hence, can be freely assigned. Henceforth, we will follow the right- 
hand rule in this case. This means that if unit vectors ij, kj_i, and 
kj are attached to axes Xj, Zj_i, and Zi, respectively, as indicated in 
Fig. 4.2b, then ij is defined as kj_i x kj. Moreover, if Zj_i and Zi are 
parallel, the location of Xi is undefined. In order to define it uniquely, 
we will specify Xi as passing through the origin of the (* — l)st frame, 
as shown in Fig. 4.2c. 

3. The distance between Zi and Zj+i is defined as Uj, which is thus 
nonnegative. 

4. The Zj-coordinate of the intersection O' of Zi with Xj+i is denoted 
by 6j. Since this quantity is a coordinate, it can be either positive 
or negative. Its absolute value is the distance between Xi and Xj+i, 
also called the offset between successive common perpendiculars. 

5. The angle between Zi and Zj+i is defined as Oj and is measured about 
the positive direction of Xj+i. This item is known as the tvh.st angle 
between successive pair axes. 

6. The angle between Xi and Xj+i is defined as Oi and is measured 
about the positive direction of Zi. 

The (n+ l)st coordinate frame is attached to the far end of the nth link. 
Since the manipulator has no (n+l)st link, the foregoing rules do not apply 
to the definition of the last frame. The analyst, thus, has the freedom to 
define this frame as it best suits the task at hand. Notice that n + 1 frames, 
Xi, X 2 , . . ., have been defined, whereas links are numbered from 0 

to n. In summary, an n-axis manipulator is composed of n + 1 links and 
n+ 1 coordinate frames. These rules are illustrated with an example below. 

Consider the architecture depicted in Fig. 4.3, usually referred to as a 
Puma robot, which shows seven links, numbered from 0 to 6, and seven 
coordinate frames, numbered from 1 to 7. Note that the last frame is arbi- 
trarily defined, but its origin is placed at a specific point of the EE, namely, 
at the operation point, P, which is used to define the task at hand. Fur- 
thermore, three axes intersect at a point C, and hence, all points of the 
last three links move on concentric spheres with respect to C, for which 
reason the subchain comprising these three links is known as a spherical 
wrist, point C being its center. By the same token, the subchain composed 
of the first four links is called the arm. Thus, the wrist is decoupled from 
the arm, and is used for orientation purposes, the arm being used for the 
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FIGURE 4.3. Coordinate frames of a Puma robot. 

positioning of point C. The arm is sometimes called the regional structure 
and the wrist the local structure, the overall manipulator thus being of the 
decoupled type. 

In the foregoing discussion, if the *th pair is R, then all quantities involved 
in those definitions are constant, except for 9i, which is variable and is thus 
termed the joint variable of the *th pair. The other quantities, i.e., a*, 6j, and 
o.i, are the joint parameters of the said pair. If, alternatively, the *th pair is 
P, then hi is variable, and the other quantities are constant. In this case, the 
joint variable is 6j, and the joint parameters are a*, Oj, and Oi. Notice that 
associated with each joint there are exactly one joint variable and three 
constant parameters. Hence, an n-axis manipulator has n joint variables — 
which are henceforth grouped in the n-dimensional vector 6, regardless of 
whether the joint variables are angular or translational — and 3n constant 
parameters. The latter define the architecture of the manipulator, while the 
former determine its configuration, or posture. 

Whereas the manipulator architecture is fully defined by its 3n Denavit- 
Hartenberg (DH) parameters, its posture is fully defined by its n joint vari- 
ables, also called its joint coordinates, once the DH parameters are known. 
The relative position and orientation between links is fully specified, then, 
from the discussions of Chapter 2, by (*) the rotation matrix taking the 
Xi, Yi, Zi axes into a configuration in which they are parallel pairwise 
to the Xi^i, Yi+i, Zi^i axes, and (m) the position vector of the origin of 
the latter in the former. The representations of the foregoing items in co- 
ordinate frame Pi will be discussed presently. First, we obtain the matrix 
representation of the rotation Qj carrying Pi into an orientation coincident 
with that of Pipi, assuming, without loss of generality because we are in- 
terested only in changes of orientation, that the two origins are coincident, 
as depicted in Fig. 4.4. This matrix is most easily derived if the rotation 
of interest is decomposed into two rotations, as indicated in Fig. 4.5. In 
that figure, Xj, Yf, Z[ is an intermediate coordinate frame P[, obtained by 
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FIGURE 4.4. Relative orientation of the fth and {i + l)st coordinate frames. 

rotating IFi about the Zi axis through an angle Oi. Then, the intermediate 
frame is rotated about Xj/ through an angle Oj, which takes it into a con- 
figuration coincident with Xj+i. Let the foregoing rotations be denoted by 
[ Cj ] j and [ Aj ] p , respectively, which are readily derived for they are in the 



canonical forms (2.55c) and (2.55a), respectively. 

Moreover, let 

Aj = cosoj, yUj = sinoj (4.1a) 

One thus has, using subscripted brackets as introduced in Section 2.2, 

cos 9i — sin 9i 0 10 0 

[Cj]. = sin6»i cos6»i 0 , [Aj]., = 0 Aj (4.1b) 

[ 0 0 ij [o /ij Aj 

and clearly, the matrix sought is computed simply as 

[Qj]j = [Cj]j[Aj]j, (4.1c) 

Henceforth, we will use the abbreviations introduced below: 

Qj = [Qj]j, Cj = [Cj]j, Aj = [Aj]j, (4.1d) 

thereby doing away with brackets, when these are self-evident. Thus, 

cos 9i — Aj sin 9i sin 9i 

Qi = [Qi]i= sin6»i AjCos6»i -/ijCos6»i (4-le) 

0 lij Aj 

One more factoring of matrix Qj, which finds applications in manipulator 
kinematics, is given below: 

Qi = ZjXj (4.2a) 
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FIGURE 4.5. (a) Rotation about axis Zi through an angle 6i\ and (b) relative 
orientation of the i'th and the (i + l)st coordinate frames. 



with Xj and Zj defined as two pure reflections, the former about the YiZi 
plane, the latter about the XiYi plane, namely, 

10 0 cos 9i sin 9i 0 

Xj = 0 — Aj Pi , Zj = sinHj — cos0j 0 (4.2b) 

0 Pi Xi 0 0 1 



Note that both Xj and Zj are symmetric and self-inverse — see Sec- 
tion 2.2. In order to derive an expression for the position vector a* con- 
necting the origin Oj of Yi with that of Jv+i, Oj+i, reference is made to 
Fig. 4.6, showing the relative positions of the different origins and axes 
involved. Prom this figure, clearly. 




(4.3a) 



where obviously. 



0 Uj 

[^j,]j= 0 , [^j+i]j+i= 0 

bi 0 



Now, in order to compute the sum appearing in eq.(4.3a), the two fore- 
going vectors should be expressed in the same coordinate frame, namely, 
JFj. Thus, 
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and hence, 



aj U = aismtfi 



(4.3b) 



For brevity, we introduce the following definition: 

aj = [aj]j (4.3c) 

Similar to the foregoing factoring of Qj, vector a* admits the factoring 

aj = Qjbj (4.3d) 

where bj is given by 



bj = hjji (4.3e) 

with the definitions introduced in eq.(4.1a). Hence, vector bj is constant 
for revolute pairs. From the geometry of Fig. 4.6, it should be apparent 
that bj is nothing but a* in Jv+i, i.e., 

bj = [aj]j^i . 

Matrices Qj can also be regarded as coordinate transformations. Indeed, 
let ij, jj, and kj be the unit vectors parallel to the Xj, 1), and Zi axes, 
respectively, directed in the positive direction of these axes. From Fig. 4.6, 
it is apparent that 

cos 9i fjLi sin Oi 

[ij+i]j= sin6»j , [kj+i]j= -/rcos6»j 
0 Aj 




FIGURE 4.6. Layout of three successive coordiuate frames. 
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whence 



[j^+i ^ h+i 



— Aj sin Oi 
Aj cos Oi 



Therefore, the components of ij+i, ji+i, and kj+i in Ti are nothing but 
the first, second, and third columns of Qj. In general, then, any vector v 
in is transformed into Ti in the form 



[v]i = [Qi]i[v]i+i 

which is a similarity transformation, as defined in eq.(2.117). Likewise, any 
matrix M in is transformed into Ti by the corresponding similarity 
transformation, as given by eq.(2.128): 

[M]i = [QiUM]i+^[Qf]i 



The inverse relations follow immediately in the form 

[v]i+i = [Qj]i[v]i, [M]i+i = [Qj]i[MUQi]i 

or upon recalling the first of definitions (4. Id), 

[v]i = Qi[v]j+i, [M]j = Qj[M]j+iQf (4.4a) 

[v]i+i = QHv]i, [M]i+i = Qj[M]iQi (4.4b) 



Moreover, if we have a chain of i frames, Ti, ■ ■ ■, Jv, then the inward 
coordinate transformation from IFi to is given by 



[v]i = Q1Q2 • • • Qi-i[v]j (4.5a) 

[M ]i = Q1Q2 • • • Qi-i[M MQ1Q2 • • • Qi-i)^ (4.5b) 

Likewise, the outward coordinate transformation takes the form 

[v]i = (Q1Q2 • • -Qi-i)^[v]i (4.6a) 

[M]i = (Q1Q2 • • • Qi-i)^[M]iQiQ2 • • • Qi-i (4.6b) 



4.3 The Kinematics of Six- Revolute Manipulators 

The kinematics of serial manipulators comprises the study of the relations 
between joint variables and Cartesian variables. The former were defined in 
Section 4.2 as those determining the posture of a given manipulator, with 
one such variable per joint; a six-axis manipulator, like the one displayed in 
Fig. 4.7, thus has six joint variables, <?i, 82 , ■ ■ ., 0^. The Cartesian variables 
of a manipulator, in turn, are those variables defining the pose of the EE; 
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since six independent variables are needed to define the pose of a rigid 
body, the manipulator of Fig. 4.7 thus contains six Cartesian variables. 

The study outlined above pertains to the geometry of the manipulator, 
for it involves one single pose of the EE. Besides geometry, the kinematics of 
manipulators comprises the study of the relations between the time-rates of 
change of the joint variables, referred to as the joint rates, and the twist of 
the EE. Additionally, the relations between the second time-derivatives of 
the joint variables, referred to as the joint accelerations, with the time-rate 
of change of the twist of the EE are also studied in this chapter. 

In this section and in Section 4.4 we study the geometry of manipulators. 
In this regard, we distinguish two problems, commonly referred to as the 
direct and the inverse kinematic problems, or DKP and correspondingly, 
IKP, for brevity. In the DKP, the six joint variables of a given six-axis 
manipulator are assumed to be known, the problem consisting of finding 
the pose of the EE. In the IKP, on the contrary, the pose of the EE is given, 
while the six joint variables that produce this pose are to be found. 

The DKP reduces to matrix multiplications, and as we shall show pres- 
ently, poses no major problem. The IKP, however, is more challenging, 
for it involves intensive variable-elimination and nonlinear-equation solv- 
ing. Indeed, in the most general case, the IKP amounts to eliminating five 
out of the six unknowns, with the aim of reducing the problem to a single 
monovariate polynomial of up to 16th degree. While finding the roots of a 
polynomial of this degree is no longer an insurmountable task, reducing the 
underl}4ng system of nonlinear equations to a monovariate pol}momial re- 
quires intensive computer-algebra work that must be very carefully planned 
to avoid the introduction of spurious roots and, with this, an increase in 
the degree of that polynomial. For this reason, we limit this chapter to the 
study of the geometric IKP of decoupled six-axis manipulators, to be de- 
fined presently. The IKP of the most general six-revolute serial manipulator 
is studied in Chapter 8. 

In stud}4ng the DKP of six-axis manipulators, we need not limit ourselves 
to a particular architecture. We thus study here the DKP of general manip- 
ulators, such as the one sketched in Fig. 4.7. This manipulator consists of 
seven rigid bodies, or links, coupled by six revolute joints. Correspondingly, 
we have seven frames, tF\, ■ ■ ■, the *th frame fixed to the (* — l)st 
link, T\ being termed the base frame, because it is fixed to the base of the 
manipulator. Manipulators with joints of the prismatic type are simpler to 
study and can be treated using correspondingly simpler procedures. 

A line £j is associated with the axis of the *th revolute joint, and a 
positive direction along this line is defined arbitrarily through a unit vector 
6j. For a prismatic pair, a line £j can be also defined, as a line having the 
direction of the pair but whose location is undefined; the analyst, then, has 
the freedom to locate this axis conveniently. Thus, a rotation of the *th link 
with respect to the (* — l)st link or correspondingly, of Jv+i with respect 
to IFi, is totally defined by the geometry of the said link, i.e., by the DH 
parameters a*, hi, and Oj, plus e* and its associated joint variable Oi. Then, 
the DH parameters and the joint variables define uniquely the posture of 
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FIGURE 4.7. Serial six-axis manipulator. 

the manipulator. In particular, the relative position and orientation of 
with respect to Ti is given by matrix Qj and vector a*, respectively, which 
were defined in Section 4.2 and are displayed below for quick reference: 

cos di — Aj sin di fjLi sin Oi a,i cos 9i 

Qj = sinHj AjCos^i — p,jCos<li , a* = ajSinUi (4-7) 

0 /Tj Aj 6j 

Thus, Qj and aj denote, respectively, the matrix rotating into an 
orientation coincident with that of and the vector joining the origin 
of with that of Jv+i, directed from the former to the latter. Moreover, Qj 
and aj, as given in eq.(4.7), are represented in coordinates. The equations 
leading to the kinematic model under study are known as the kinematic 
displacement equations. It is noteworthy that the problem under study is 
equivalent to the input-output analysis problem of a seven-revolute linkage 
with one degree of freedom and one single kinematic loop (Duffy, 1980). 
Because of this equivalence with a closed kinematic chain, sometimes the 
displacement equations are also termed closure equations. These equations 
relate the orientation of the EE, as produced by the joint coordinates, with 
the prescribed orientation Q and the position vector p of the operation 
point P of the EE. That is, the orientation Q of the EE is obtained as 
a result of the six individual rotations { Qj }® about each revolute axis 
through an angle 0j, in a sequential order, from 1 to 6. If, for example, the 
foregoing relations are expressed in P\, then 

[Q6]i[Q5]i[Q4]i[Q3]i[Q2]i[Qi]i = [Q]i (4.8a) 
[ai ]i + [^2 ]i + [aa ]i + [a4 ]i + [as ]i + [as ]i = [p ]i (4.8b) 

Notice that the above equations require that all vectors and matrices 
involved be expressed in the same coordinate frame. However, we derived 
in Section 4.2 general expressions for Qj and aj in Pi, eqs.(4.1e) and (4.3b), 
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respectively. It is hence convenient to represent the foregoing relations in 
each individual frame, which can be readily done by means of similarity 
transformations. Indeed, if we apply the transformations (4.5a & b) to each 
of [aj]i and [Qi]i, respectively, we obtain a* or correspondingly, Qj, in 
Ti- Therefore, eq.(4.8a) becomes 

[Qi]i[Q 2 ] 2 [Q 3 ] 3 [Q 4 ] 4 [Q 5 ] 5 [Q 6]6 = [Q]l 

Now for compactness, let us represent [Q]i simply by Q and let us recall 
the abbreviated notation introduced in eq.(4.1d), whereby [Qj ]j is denoted 
simply by Qj, thereby obtaining 

Q1Q2Q3Q4Q5Q6 = Q (4.9a) 

Likewise, eq.(4.8b) becomes 

ai + Qi(a 2 + Q 2 a 3 + Q2Qs^4 + Q2Q3Q4as + Q2Q3Q4Q5^e) = P (4.9b) 



in which both sides are given in base-frame coordinates. Equations 
(4.9a & b) above can be cast in a more compact form if homogeneous 
transformations, as defined in Section 2.5, are now introduced. Thus, if we 
let Tj = { Tj }i be the 4x4 matrix transforming J^j+i-coordinates into Ti~ 
coordinates, the foregoing equations can be written in 4 x 4 matrix form, 
namely, 

T 1 T 2 T 3 T 4 T 5 T 6 = T (4.10) 



with T denoting the transformation of coordinates from the end-effector 
frame to the base frame. Thus, T contains the pose of the end-effector. 

In order to ease the discussion ahead, we introduce now a few defini- 
tions. A scalar, vector, or matrix expression is said to be multilinear in 
a set of vectors {vj})^ if those vectors appear only linearly in the same 
expression. This does not prevent products of components of those vectors 
from occurring, as long as each product contains only one component of 
the same vector. Alternatively, we can say that the expression of interest 
is multilinear in the aforementioned set of vectors if and only if the partial 
derivative of that expression with respect to vector Vj is independent of 
Vj, for i = 1, . . . , A^. For example, every matrix Qj and every vector a*, 
defined in eqs.(4.1e) and (4.3b), respectively, is linear in vector Xj, where 
Xj is defined as 



Xj 



cos Oi 
sin Oi 



(4.11) 



Moreover, the product Q1Q2Q3Q4Q5Q6 appearing in eq.(4.9a) is hexalin- 
ear, or simply, multilinear^ in vectors {xj }®. Likewise, the sum appearing 
in eq.(4.9b) is multilinear in the same set of vectors. By the same token, 
a scalar, vector, or matrix expression is said to be multiquadratic in the 
same set of vectors if those vectors appear at most quadratically in the 
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said expression. That is, the expression of interest may contain products 
of the components of all those vectors, as long as those products contain, 
in turn, a maximum of two components of the same vector, including the 
same component squared. Qualifiers like multicubic, muUiquartic, etc., bear 
similar meanings. 

Further, we partition matrix Qj rowwise and columnwise, namely. 



Qi 



Pi q* Uj 



(4.12) 



It is pointed out that the third row of Qj, of, is independent of 0j, a fact 
that will be found useful in the forthcoming derivations. Furthermore, note 
that according to the DH notation, the unit vector e* in the direction of 
the ith joint axis in Fig. 4.7 has JQ-components given by 




(4.13) 



Henceforth, e is used to represent a 3-dimensional array with its last com- 
ponent equal to unity, its other components vanishing. Thus, we have 

QiOi = Qfuj = e (4.14a) 



or 

Uj = Qje, Oj = Qfe (4.14b) 

That is, if we regard e in the first of the foregoing relations as [ej+i ]j+i, and 
as [ej]i in the second relation, then, from the coordinate transformations 
of eqs.(4.4a & b). 



Uj [uj^i]j, and Oj [oj]j^i 



(4.15) 



4.4 The IKP of Decoupled Manipulators 

Industrial manipulators are frequently supplied with a special architecture 
that allows a decoupling of the positioning problem from the orientation 
problem. In fact, a determinant design criterion in this regard has been 
that the manipulator be kinematically invertible, i.e., that it lend itself to 
a closed-form inverse kinematics solution. Although the class of manipula- 
tors with this feature is quite broad, we will focus on a special kind, the most 
frequently encountered in commercial manipulators, that we will term de- 
coupled. Decoupled manipulators were defined in Section 4.2 as those whose 
last three joints have intersecting axes. These joints, then, constitute the 
wrist of the manipulator, which is said to be spherical, because when the 
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FIGURE 4.8. A general 6R manipulator with decoupled architecture. 

point of intersection of the three wrist axes, (U, is kept fixed, all the points 
of the wrist move on spheres centered at C. In terms of the DH parameters 
of the manipulator, in a decoupled manipulator 04 = 05 = 65 = 0, and 
thus, the origins of frames 5 and 6 are coincident. All other DH parameters 
can assume arbitrary values. A general decoupled manipulator is shown in 
Fig. 4.8, where the wrist is represented as a concatenation of three revolutes 
with intersecting axes. 

In the two subsections below, a procedure is derived for determining all 
the inverse kinematics solutions of decoupled manipulators. In view of the 
decoupled architecture of these manipulators, we study their inverse kine- 
matics by decoupling the positioning problem from the orientation problem. 

4.4.1 The Positioning Problem 

The inverse kinematics of the robotic manipulators under study begins by 
decoupling the positioning and orientation problems. Moreover, we must 
solve first the positioning problem, which is done in this subsection. 

Let C denote the intersection of axes 4, 5, and 6, i.e., the center of the 
spherical wrist, and let c denote the position vector of this point. Clearly, 
the position of C is independent of joint angles <?4, <?5, and Oq] hence, only 
the first three joints are to be considered for this analysis. The arm structure 
depicted in Fig. 4.9 will then be analyzed. From that figure, 

+ Qia2 + QiQ2a3 + QiQ2Q3a4 = c (4.16) 

where the two sides are expressed in JFj^-coordinates. This equation can be 
readily rewritten in the form 

»2 + Q2»3 + Q2Qaa4 = Qf (c - ai) 
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or if we recall eq.(4.3d), 



Q2(b2 + Qaba + QaQ4b4) Q^c — bi 
However, since we are dealing with a decoupled manipulator, we have 



a .4 = Q4b4 = 0 = 646 

_h_ 

which has been rewritten as the product of constant 64 times the unit vector 
e defined in eq.(4.13). 

Thus, the product QaQ 4 b 4 reduces to 

QaQ 4 b 4 = & 4 Qae = 64 ua 



with Uj defined in eq.(4.14b). Hence, eq.(4.16) leads to 
Q 2 (b 2 + Qaba + &4Ua) = Qfc - bi 



(4.17) 



Further, an expression for c can be derived in terms of p, the position 
vector of the operation point of the EE, and Q, namely. 



c p — QiQ2QaQ4a5 — QiQ2QaQ4Q5ae 



(4.18a) 



Now, since 05 = 65 = 0, we have that as = 0, eq.(4.18a) thus yielding 



c = p - QQi^ae = p - Qbe 



(4.18b) 



Moreover, the base coordinates of P and C, and hence, the J^i-components 
of their position vectors p and c, are defined as 



X xc 

[p]i = y , [c]i= yc 

_z\ [ Zc _ 

so that eq.(4.18b) can be expanded in the form 

Xc X - {qnaa + qi2haHfi + qnhfiXfi) 

yc = y — (</ 2 ia 6 + </ 22 & 6 M 6 + </ 2 a& 6 ^ 6 ) (4.18c) 

_zc\ [ 2 ; - ('i'aiae + 'i'aa&eMe + '?aa&6Ae)_ 

where qij is the (*, j) entry of [Q]i, and the positioning problem now be- 
comes one of finding the first three joint angles necessary to position point 
C* at a point of base coordinates xc, yc, and zc- We thus have three un- 
knowns, but we also have three equations at our disposal, namely, the three 
scalar equations of eq.(4.17), and we should be able to solve the problem 
at hand. 

In solving the foregoing system of equations, we first note that (*) the 
left-hand side of eq.(4.17) appears multiplied by Q 2 ; and (n) O 2 does not 
appear in the right-hand side. This implies that (*) if the Euclidean norms 
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FIGURE 4.9. Three-axis, serial, positioning manipulator. 

of the two sides of that equation are equated, the resulting equation will 
not contain O2] and (n) the third scalar equation of the same equation is 
independent of 62, by virtue of the structure of the Qj matrices displayed 
in eq.( 4 . 1 e). Thus, we have two equations free of 62, which allows us to 
calculate the two remaining unknowns 9 i and 9 ^. 

Let the Euclidean norm of the left-hand side of eq.( 4 . 17 ) be denoted by 
I, that of its right-hand side by r. We then have 

= 02 + &2 T T ^3 T ^4 T Q3b3 + 2&4b^ 113 + 2A36364 

r^^llcf + ||bif -2bfQrc 

from which it is apparent that P is linear in X3 and is linear in xi, for 
Xj defined in eq.( 4 . 11 ). Upon equating P with r^, then, an equation linear 
in xi and X3 — not bilinear in these vectors — is readily derived, namely. 



Aci + Bsi + ( 7 c 3 + Ds 2 j + E = 0 ( 4 . 19 a) 

whose coefficients do not contain any unknown, i.e., 

A = 2 aixc ( 4 . 19 b) 

B = 2 a\yc ( 4 . 19 c) 

C = 20303 — 262&4M2M3 ( 4 . 19 d) 

D = 20362M2 + 20264M3 ( 4 . 19 e) 

E = al+ al + hl + hl + hl- a\ - - yl; - {zc - hif 

+262^3^2 + 262&4A2A3 + 26364A3 ( 4 . 19 f) 
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Moreover, the third scalar equation of eq.(4.17) takes on the form 



Fci + Gsi + Hc^ + /s3 + J = 0 (4.20a) 

whose coefficients, again, do not contain any unknown, as shown below: 

F = yclJ-i (4.20b) 

G = —xcyi'i (4.20c) 

H = -64M2M3 (4.20d) 

I = a,3H2 (4.20e) 

J = 62 + + &4A2A3 — (zc — &i)Ai (4.20f) 



Thus, we have derived two nonlinear equations in 9i and 0^ that are 
linear in ci, si, C3, and S3. Each of these equations thus defines a contour 
in the 0i-<?3 plane, their intersections determining all real solutions to the 
problem at hand. 

Note that if Cj and Sj are substituted for their equivalents in terms 
of tan(0j/2), for * = 1,3, then two biquadratic pol}momial equations in 
tan(<?i/2) and tan(<?3/2) are derived. Thus, one can eliminate one of these 
variables from the foregoing equations, thereby reducing the two equations 
to a single quartic polynomial equation in the other variable. The quartic 
equation thus resulting is called the characteristic equation of the problem 
at hand. Alternatively, the two above equations, eqs.(4.19a) and (4.20a), 
can be solved for, say, ci and si in terms of the data and C3 and S3, namely. 



Cl 



Si 



—G[Gc^ + Ds^ + E) + B(Hc^ + Is^ + J) 

a; 

F{Gc3 + Ds3 + E)~ A{Hc3 + /S3 + J) 

Ai 



with Ai defined as 



Ai — AG — FB — —2aiiai{xjj + y^) 



(4.21a) 

(4.21b) 



(4.21c) 



Note that in trajectory planning, to be studied in Chapter 5, Ai can be 
computed off-line, i.e., prior to setting the manipulator into operation, for 
it is a function solely of the manipulator parameters and the Cartesian co- 
ordinates of a point lying on the path to be tracked. Moreover, the above 
calculations are possible as long as Ai does not vanish. Now, Ai vanishes if 
and only if any of the factors ai, p,i, and +1/^ does. The first two condi- 
tions are architecture-dependent, whereas the third is position-dependent. 
The former occur frequently in industrial manipulators, although not both 
at the same time. If both parameters ai and p,i vanished, then the arm 
would be useless to position arbitrarily a point in space. The third condi- 
tion, i.e., the vanishing of +yc, means that point G lies on the Z\ axis. 
Now, even if neither ai nor p,i vanishes, the manipulator can be positioned 
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in a configuration at which point C lies on the Z\ axis. Such a configura- 
tion is termed the first singularity. Note, however, that with point C being 
located on the Z\ axis, any motion of the first joint, with the two other 
joints locked, does not change the location of C . For the moment, it will be 
assumed that Ai does not vanish, the particular cases under which it does 
being studied later. Next, both sides of eqs.(4.21a & b) are squared, the 
squares thus obtained are then added, and the sum is equated to 1 , which 
leads to a quadratic equation in X 3 , namely. 



iFCg + TSg + AfcgSg + Wcg + Usg + Q — 0 


(4.22) 


whose coefficients, after simplification, are given below: 




K = AalH^ + pIC^ 


(4.23a) 


L = AalP + 


(4.23b) 


M = 2{Aa\HI + p\CD) 


(4.23c) 


N = 2{Aa\HJ + p\CE) 


(4.23d) 


P = 2{Aa\lJ + p\DE) 


(4.23e) 


Q = Aa\P + ~ Aa\p\p^ 


(4.23f) 



with defined as 

2 _ 2 I 2 

P — Xq + Pq 

Now, two well-known trigonometric identities are introduced, namely, 

Cg = I Sg = where T 3 = tan(^) (4.24) 

1 “T 1 “T Z 

Henceforth, the foregoing identities will be referred to as the tan-half- angle 
identities. We will be resorting to them throughout the book. Upon sub- 
stitution of the foregoing identities into eq.(4.22), a quartic equation in Tg 



is obtained, i.e., 

Rt^ + S't| + Tt| + [/t3 + U = 0 (4.25) 

whose coefficients are all computable from the data. After some simplifica- 
tions, these coefficients take on the forms 

R = Aa\{J — HY + ~ — Ap^a\Yi (4.26a) 

S = A[Aa\l{J - H) + p\D{E -C)] (4.26b) 

T = 2[4a?(j2 + 2P) + p\{E^ - + 2DY 

—Ap'^a^pl] (4.26c) 

U = A[Aall{H + J) + pID{C + E)] (4.26d) 

V = 4af(J + HY +pI(E + CY - Ap'^ajpl (4.26e) 



Furthermore, let { (rg)* }f be the four roots of eq.(4.25). Thus, up to four 
possible values of 0 g can be obtained, namely, 

(6»g)j = 2arctan[(Tg)j], * = 1,2, 3,4 



(4.27) 
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Once the four values of 9 ^ are available, each of these is substituted into 
eqs.( 4 . 21 a & b), which thus produce four different values of 9 \. For each 
value of 9 \ and <?3, then, one value of 62 can be computed from the first 
two scalar equations of eq.( 4 . 17 ), which are displayed below: 

All cos + Ai2 sin 62 = xc cos 9 i + yc sin 9 i — ai ( 4 . 28 a) 

—Ai2 cos 62 + All sin 62 = — xcAi sin 9 i + yc^i cos 9 i 

+ (zc-6i)mi ( 4 . 28 b) 

where 

All = ci2 + 03 cos <?3 + 64yU,3 sin 9 ^ ( 4 . 28 c) 

Ai2 = — CI3A2 sin <?3 + 63/U2 + &4A2P-3 cos <?3 + 64M2 A3 ( 4 . 28 d) 

Thus, if All and A12 do not vanish simultaneously, angle <?2 is readily 
computed in terms of 9 i and 9 s from eqs.( 4 . 28 a & b) as 

cos 6»2 = -^lAii(xc cos9i + yc sin 9i - ai) 

^2 

-Ai2 [-Xc Ai sin 9 i + ycM cos 9 i 
+ (zc - bi)fj.ij} ( 4 . 29 a) 

sin 6»2 = — {Ai 2 (xc cos 6»i + yc sin 6»i - ai ) 

^2 

+ All [-XC Ai sin 9 i + yc Ai cos 9 i 
+ {zc — bi)jji]} ( 4 . 29 b) 

where A2 is defined as 

A2 = Ah + Ah 

= 02 + ag(cos^ 9 s + A2 sin^ Hg) + 64(03 (sin^ 0g + Ag cos^ 0g) 

+ 20203 cos 9 s + 20264(03 sin 0g 
+ 2A2(02(6g + 64 Ag )( 64(03 cos 6»g - OgSin^g) 

+20364(1 - A2)(03sin6»3C0s6»3 + (63 + Ag64)^(02 ( 4 . 29 c) 

the case in which Ag = 0, which leads to what is termed here the second 
singularity, being discussed presently. 

Takano ( 1985 ) considered the solution of the positioning problem for 
all possible combinations of prismatic and revolute pairs in the regional 
structure of a manipulator, thereby finding that 

1 . In the case of arms containing either three revolutes, or two revolutes 
and one prismatic pair, with a general layout in all cases, a quartic 
equation in cos0g was obtained; 

2. in the case of one revolute and two prismatic pairs, the positioning 
problem was reduced to a single quadratic equation, the problem at 
hand thus admitting two solutions; 
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3. finally, for three prismatic pairs, a single linear equation was derived, 
the problem thus admitting a unique solution. 



The Vanishing of Ai 

In the above derivations we have assumed that neither p,i nor ai vanishes. 
However, if either p,i = 0 or ai = 0, then one can readily show that eq.(4.25) 
reduces to a quadratic equation, and hence, this case differs essentially 
from the general one. Note that one of these conditions can occur, and the 
second occurs indeed frequently, but both together never occur, because 
their simultaneous occurrence would render the manipulator useless for a 
three-dimensional task. We thus have the two cases discussed below: 



1. yUi = 0, ai 7^ 0. In this case, one has 



A, B 0, F = G = 0 

Under these conditions, eq. (4.20a) and the tan-half-angle identities 
given in eq.(4.24) yield 

( J - F)r| + 2 /t3 + {J + H)=0 



which thus produces two values of T3, namely. 



(■^3)1,2 



-I ± V/2 -J^+H^ 
J -H 



(4.30a) 



Once two values of 0^ have been determined according to the above 
equation, 9i can be found using eq.(4.19a) and the tan-half-angle 
identities, thereby deriving 

{E' - A)tI + 2Bti + {E' + A) =Q 

where 

E' = Cc^ + Ds^ E E, Ti = tan 



whose roots are 



in) 1,2 



-B ± VB2 - U'2 -b M2 
E' -A 



(4.30b) 



Thus, two values of 9i are found for each of the two values of 0^, 
which results in four positioning solutions. Values of 02 are obtained 
using eqs.(4.29a & b). 



2. a\ = 0, yUi 7^ 0. In this case, one has an architecture similar to that 
of the robot of Fig. 4.3. We have now 




4.4 The IKP of Decoupled Manipulators 



125 



A = B = 0, F, G ^ 0 



Under the present conditions, eq.(4.19a) reduces to 
(E - C)tI + 2Dt^ + {E + C)=0 



which produces two values of T 3 , namely, 



(■^ 3 ) 1,2 



-D ± ^£*2 -E^+C^ 
E-C 



(4.31a) 



With the two values of <?3 obtained, 0 \ can be found using eq. (4.20a) 
and the tan-half-angle identities to produce 

( J' - F)tI + 2Gn + ( J' + U) = 0 



where 



J' 



whose roots are 



iJc 3 + /s 3 + J, Ti = tan 



2 



(d)i,2 



-G± VG2 -J’^ + F^ 
E -F 



(4.31b) 



Once again, the solution results in a cascade of two quadratic equa- 
tions, one for 9 ^ and one for <?i, which yields four positioning solutions. 
As above, 62 is then determined using eqs. (4.29a & b). Note that for 
the special case of the manipulator of Fig. 4.3, we have 

ai = 62 = 0, Q!i = 03 = 90°, Q !2 = 0° 

and hence, 

H = I = 0 , A = U2 + CI3 + 63 + 64 — [xp + r/p + (zc — &i)^] , 

C = 2a2U3, D = 2a2&4, F = yc, G = -xc, J = 

In this case, the foregoing solutions reduce to 

, , -D ± VU2 + D^-E^ . . xc±^xl+yl-bl 

(■^3)1,2- ^ , (D)i,2- T 

E-C b3-yc 

A robot with the architecture studied here is the Puma, which is dis- 
played in Fig. 4.10 in its four distinct postures for the same location of its 
wrist center. Notice that the orientation of the EE is kept constant in all 
four postures. 




126 



4. Kinetostatics of Simple Robotic Manipulators 






(b) 




(d) 



FIGURE 4.10. The four arm configurations for the positioning problem of the 
Puma robot: (a) and (b), elbow down; (a) and (c), shoulder fore; (c) and (d), 
elbow up; (b) and (d), shoulder aft. 



The Vanishing of A 2 

In some instances, A 2 , as defined in eq. (4.29c), may vanish, thereby pre- 
venting the calculation of O 2 from eqs.(4.29a & b). This posture, termed 
the second singularity, occurs if both coefficients An and An of eqs.(4.28a 
& b) vanish. Note that from their definitions, eqs.(4.28c & d), these co- 
efficients are not only position- but also architecture-dependent. Thus, an 
arbitrary manipulator cannot take on this configuration unless its geometric 
dimensions allow it. This type of singularity will be termed architecture- 
dependent, to distinguish it from others that are common to all robots, 
regardless of their particular architectures. 

We can now give a geometric interpretation of the singularity at hand: 
First, note that the right-hand side of eq.(4.17), from which eqs. (4.28a & b) 
were derived, is identical to (c — ai), which means that this expression is 

nothing but the JF 2 -representation of the position vector of C. That is, the 
components of vector Qf(c — ai) are the JF 2 -components of vector O 2 C. 
Therefore, the right-hand sides of eqs. (4.28a & b) are, respectively, the X 2 - 
and l 2 -components of vector O 2 C ■ Consequently, if An = An = 0, then 
the two foregoing components vanish and, hence, point C lies on the Z 2 
axis. The first singularity thus occurs when point C lies on the axis of the 
first revolute, while the second occurs when the same point lies on the axis 
of the second re volute. 
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Many industrial manipulators are designed with an orthogonal architec- 
ture, which means that the angles between neighbor axes are multiples 
of 90°. Moreover, with the purpose of maximizing their workspace, or- 
thogonal manipulators are designed with their second and third links of 
equal lengths, thereby rendering them vulnerable to this type of singular- 
ity. An architecture common to many manipulators such as the Cincinnati- 
Milacron, ABB, Fanuc, and others, comprises a planar two-axis layout with 
equal link lengths, which is capable of turning about an axis orthogonal to 
these two axes. This layout allows for the architecture singularity under 
discussion, as shown in Fig. d.ffa. The well-known Puma manipulator is 
similar to the aforementioned manipulators, except that it is supplied with 
what is called a shoulder offset 63, as illustrated in Fig. 4.3. This offset, 
however, does not prevent the Puma from attaining the same singularity 
as depicted in Fig. d.ffb. Note that in the presence of this singularity, angle 
O 2 is undetermined, but 0\ and are determined in the case of the Puma 
robot. Flowever, in the presence of the singularity of Fig. d.ffa, neither 0\ 
nor O 2 are determined; only <?3 of the arm structure is determined. 

Example 4.4.1 A manipulator with a common orthogonal architecture is 
displayed in Fig. 4-12 in an arbitrary configuration. The arm architecture 
of this manipulator has the DFl parameters shown below: 

ai =0, = 0, Q!i = 90°, Q!2 = 0° 

Find its inverse kinematics solutions. 

Solution: A common feature of this architecture is that it comprises 02 = 64 . 
In the present discussion, however, the latter feature need not be included, 
and hence, the result that follows applies even in its absence. In this case. 





FIGURE 4.11. Architecture-dependent singularities of (a) the Cincinnati— 
Milacron and (b) the Puma robots. 
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FIGURE 4.12. An orthogonal decoupled manipulator. 

coefficients (U, D, and E take on the forms 

C = 20203, D = 0 , E = a\+ a^ — + z^) 

Hence, 

E — C = (o2 — 03)^ — {x\;+yjj + z^), E+C = (02 + 03)^ — (x^ + y^ + z^) 
Moreover, 



H = I= J = 0 



and so 



J' = 0, E = yc, G = -xc 
The radical of eq. (4.31b) reduces to x^ + y^. Thus, 

tan ( — ^ - a:c ± Y^x^ +y^ ^ -1 ± y^l + {yclxc^ 
V2y -yc yc/xc 

Now we recall the relation between tan(<?i/2) and tan<?i, namely. 



(4.32a) 



(4.32b) 



, — 1 ± Vl + tan^ 9 1 

tan I — = 

2 J tan 0i 

Upon comparison of eqs. (4.32a) and (4.32b), it is apparent that 



9i = arctan ( — 

\xc 

a result that can be derived geometrically for this simple arm architecture. 
Given that the arctan(-) function is double- valued, its two values differing 
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t*’ 




FIGURE 4.13. An orthogonal RRR manipulator. 



in 180°, we obtain here, again, two values for 0i. On the other hand, 0^ is 
calculated from eq. (4.31a) as 



(o)l,2 



, V02 - 
^ E-C 



thereby obtaining two values of . As a consequence, the inverse position- 
ing problem of this arm architecture admits four solutions as well. These 
solutions give rise to two pairs of arm postures that are usually referred to 
as elhow-up and elbow-down. 



Example 4.4.2 Find all real inverse kinematic solutions of the manip- 
ulator shown in Fig. f.l3, when point C of its end-effector has the base 
coordinates 0(0, 2a, —a). 

Solution: The Denavit-Hartenberg parameters of this manipulator are de- 
rived from Fig. 4.14, where the coordinate frames involved are indicated. 
In defining the coordinate frames of that figure, the Denavit-Hartenberg 
notation was followed, with defined, arbitrarily, as parallel to Z 3 . Prom 
Fig. 4.14, then, we have 



ai — U 2 — U 3 — 62 — ^3 — ^7 — ^4 — d? Oil — Oi 2 — 90°, 03 — 0° 

One inverse kinematic solution can be readily inferred from the geom- 
etry of Fig. 4.14. For illustration purposes, and in order to find all other 
inverse kinematic solutions, we will use the procedure derived above. To this 




130 



4. Kinetostatics of Simple Robotic Manipulators 




FIGURE 4.14. The coordinate frames of the orthogonal RRR manipulator. 

end, we first proceed to calculate the coefficients of the quartic polynomial 
equation, eq.(4.25), which are given, nevertheless, in terms of coefficients 
K, Q of eqs.(4.23a-f). These coefficients are given, in turn, in terms 
of coefficients A, J of eqs.(4.19b-f) and (4.20b-f). We then proceed to 
calculate all the necessary coefficients in the proper order: 

M = 0, B = 4a^, C = D = -E = 2(? 

F = 2a, G = H = 0, I = J = a 

Moreover, 

K = 4a*^, L = 8a^, M = 8a^, N = —8a^, P = 0, Q = —8a^, 
The set of coefficients sought thus reduces to 

R = K-N + Q = 4:a‘^ 

S = 2{P-M) = -16a-^ 

T = 2{Q + 2L-K) = 8a^ 

U = 2{M + P) = 16a-^ 

V = K + N + Q = -12a-^ 

which leads to the quartic equation given below: 

Tg — 4 t| + 2t| + 4t3 —3 = 0 
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with four real roots, namely, 

(t'3)i = {^3)2 = 1, (t3)3 = -1, (t3)4 = 3 

These roots yield the 9^ values that follow: 

{03)1 = (93)2 = 90°, (03)3 = -90°, (03)4 = 143.13° 

The quartic polynomial thus admits one double root, which means that at 
the configurations resulting from this root, two solutions meet, thereby pro- 
ducing a singularity, an issue that is discussed in Subsection 4.5.2. Below, 
we calculate the remaining angles for each solution: Angle 9\ is computed 
from relations (4.21a-c), where Ai = —8a®. 

The first two roots, (<^3)1 = (<^3)2 = 90°, yield C3 = 0 and S3 = 1. Hence, 
eqs. (4.21a & b) lead to 

B(/ + J) 4a®(a + a) 

^ ^ -8a® ^ 

F{D + E) 2a(2a®-2a2) ^ 

^ Ai ^ -8a® ^ 

and hence, 

{0i)i = {9 i)2 = 180° 

With 9\ known, is computed from the first two of eqs. (4. 17), namely. 



C2 = 0, S2 = -1 



and hence. 



{02)1 = {02)2 = -90° 



The remaining roots are treated likewise. These are readily calculated as 
shown below: 



(<?i )3 = -90°, (02)3 = 0, (01)4 = 143.13°, (02)4 = 0 

It is noteworthy that the architecture of this manipulator does not allow 
for the second singularity, associated with A2 = 0. 

Example 4.4.3 For the same manipulator of Example f.f.2, find all real 
inverse kinematic solutions when point C of its end-effector has the base 
coordinates C{0, a, 0), as displayed in Fig. f.l5. 

Solution: In this case, one obtains, successively, 

A = 0, B = C = D = E = 2a^, 

F = a, G = 0 F[ = 0, I = J = a 

K = 4a®, L = M = N = 8a®, P = 16a®, Q = 4a® 

R = 0, S = 16a®, T = 32a®, U = 48a®, V = 16a® 
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FIGURE 4.15. Manipulator configuration for G(0, a, 0). 

Moreover, for this case, the quartic eq.(4.22) degenerates into a cubic equa- 
tion, namely, 

uf + 2 i~3 + 3t3 + 1 = 0 
whose roots are readily found as 

( 7 - 3)1 = -0.43016, (t 3 ) 2,3 = -0.78492 + jl. 30714 

where j is the imaginary unit, i.e., j = a/— 1. That is, only one real solution 
is obtained, namely, (<^ 3)1 = —46.551°. However, shown in Fig. 4.15 is a 
quite symmetric posture of this manipulator at the given position of point C 
of its end-effector, which does not correspond to the real solution obtained 
above. In fact, the solution yielding the posture of Fig. 4.15 disappeared 
because of the use of the quartic polynomial equation in tan(<? 3 / 2 ). Note 
that if the two contours derived from eqs.(4.19a) and (4.20a) are plotted, 
as in Fig. 4.16, their intersections yield the two real roots, including the 
one leading to the posture of Fig. 4.15. 

The explanation of how the fourth root of the quartic equation disap- 
peared is given below: Let us write the quartic polynomial in full, with a 
“small” leading coefficient e, namely. 



ct3 + + 2T3 + 3t3 + 1 — 0 
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FIGURE 4.16. Contours producing the two real solutions for Example 4.4.3. 
Upon dividing both sides of the foregoing equation by Tg , we obtain 




from which it is clear that the original equation is satisfied as e ^ 0 if and 
only if T3 ^ ±oo, i.e, if 9^ = 180°. It is then apparent that the missing 
root is <?3 = 180°. The remaining angles are readily calculated as 

= -105.9°, { 82)1 = -149.35°, (<?i)4 = 180°, ( 92)4 = 180° 

4.4 The Orientation Problem 

Now the orientation inverse kinematic problem is addressed. This problem 
consists of determining the wrist angles that will produce a prescribed 
orientation of the end-effector. This orientation, in turn, is given in terms 
of the rotation matrix Q taking the end-effector from its home attitude to 
its current one. Alternatively, the orientation can be given by the natural 
invariants of the rotation matrix, vector e and angle (})■ Moreover, since <?i, 
62 , and <?3 are available, Qu Q2, and Q3 become data for this problem. 
One now has the general layout of Fig. 4.17, where angles { 9i j® are to be 
determined from the problem data, which are in this case the orientation 
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FIGURE 4.17. General architecture of a spherical wrist. 



of the end-effector and the architecture of the wrist; the latter is defined 
by angles 04 and 05, neither of which can be either 0 or tt. 

Now, since the orientation of the end-effector is given, we know the com- 
ponents of vector 65 in any coordinate frame. In particular, let 



[ee]4 



e 

rj 

c 



( 4 . 33 ) 



Moreover, the components of vector 65 in Ta are nothing but the entries 
of the third column of matrix Q4, i.e.. 



[es ]a 



jj,4 sin 04 
-p,4 cos 64 
A4 



( 4 . 34 ) 



Furthermore, vectors 65 and make an angle 05, and hence, 

e|’e5 = A5 or [eejJ [05 (4 = A5 ( 4 . 35 ) 

Upon substitution of eqs.( 4 . 33 ) and ( 4 . 34 ) into eq.( 4 . 35 ), we obtain 

^p,4 sin 04 — rjj44 cos 04 + ((A4 = A5 ( 4 . 36 ) 



which can be readily transformed, with the aid of the tan-half-angle iden- 
tities, into a quadratic equation in T4 = tan(<?4/2), namely, 

(A5 - riiJ,4 - CA4)t| - 2^/44 T 4 + (A5 + 47/44 - CA4) = 0 



( 4 . 37 ) 
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its two roots being given by 






^464 ± + T?^)M4 ~ (^5 - C^4)^ 

As — CA 4 — ?74t4 



(4.38) 



Note that the two foregoing roots are real as long as the radical is positive, 
the two roots merging into a single one when the radical vanishes. Thus, a 
negative radical means an attitude of the EE that is not feasible with the 
wrist. It is to be pointed out here that a three-revolute spherical wrist is 
kinematically equivalent to a spherical joint. However, the spherical wrist 
differs essentially from a spherical joint in that the latter has, kinematically, 
an unlimited workspace — a physical spherical joint, of course, has a limited 
workspace by virtue of its mechanical construction — and can orient a rigid 
body arbitrarily. Therefore, the workspace W of the wrist is not unlimited, 
but rather defined by the set of values of ry, and ( that satisfy the two 
relations shown below: 



e+v"+C" = l (4.39a) 

/(e, V, C) = + rf)A - (As - CA 4 )' > 0 (4.39b) 

In view of condition (4.39a), however, relation (4.39b) simplifies to an in- 
equality in Q alone, namely, 

E(C) = C'-2A4AsC-(M4-Ai)<0 (4.40) 

As a consequence, 

1 . W is a region of the unit sphere S centered at the origin of the three- 
dimensional space; 

2 . W is bounded by the curve = 0 on the sphere; 

3. the wrist attains its singular configurations along the curve = 0 
lying on the surface of S. 

In order to gain more insight on the shape of the workspace W, let us 
look at the boundary defined by F{Q = 0. Upon setting F{Q to zero, we 
obtain a quadratic equation in (), whose two roots can be readily found to 
be 

Cl, 2 = A4A5 ± 1414415! ( 4 - 41 ) 

which thus defines two planes, ili and il 2 , parallel to the ^-17 plane of the 
three-dimensional space, intersecting the C-axis at Ci and C 2 , respectively. 
Thus, the workspace W of the spherical wrist at hand is that region of the 
surface of the unit sphere S contained between ili and il 2 . For example, a 
common wrist design involves an orthogonal architecture, i.e., 0:4 = 0:5 = 
90°. For such wrists. 



Cl , 2 = ±1 
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and hence, orthogonal wrists become singular when [e 6]4 = [0, 0, ±1]^, 
i.e., when the fourth and the sixth axes are aligned. Thus, the workspace 
of orthogonal spherical wrists is the whole surface of the unit sphere cen- 
tered at the origin, the singularity curve thus degenerating into two points, 
namely, the two intersections of this sphere with the (J'-axis. If one views 
(J' = 0 as the equatorial plane, then the two singularity points of the 
workspace are the poles. 

An alternative design is the so-called three-roll wrist of some Cincinnati- 
Milacron robots, with 04 = 05 = 120°, thereby leading to A 4 = A 5 = — 1/2 
and p ,4 = p ,5 = %/3/2. For this wrist, the two planes Ui and II 2 are found 
below: First, we note that with the foregoing values. 

Cl , 2 = 1, 

and hence, the workspace of this wrist is the part of the surface of the unit 
sphere S that lies between the planes II i and II 2 parallel to the ^-17 plane, 
intersecting the /-axis at /i = 1 and Q 2 = respectively. Hence, if 

/ = 0 is regarded as the equatorial plane, then the points of the sphere S 
that are outside of the workspace of this wrist are those lying at a latitude 
of less than —30°. The singularity points are thus the north pole and the 
parallel of latitude —30°. 

Once <?4 is calculated from the two foregoing values of T 4 , if these are 
real, angle 0^ is obtained uniquely for each value of <? 4 , as explained below: 
First, eq.(4.9a) is rewritten in a form in which the data are collected in the 
right-hand side, which produces 

Q 4 Q 5 Q 6 = R- (4.42a) 

with R defined as 

R=Q 3 Q^QrQ (4.42b) 

Moreover, let the entries of R in the fourth coordinate frame be given as 



rii 


ri2 


ri3 


T21 


r-22 


r-23 


^31 


r-32 


r33 



Expressions for 9^ and 9^ can be readily derived by solving first for Q 5 
from eq.(4.42a), namely, 

Q5 = QIRQ/^ (4.43) 

Now, by virtue of the form of the Qj matrices, as appearing in eq.(4.1e), 
it is apparent that the third row of Q j does not contain 9i . Hence, the third 
column of the matrix product of eq.(4.43) is independent of 9q. Thus, two 
equations for 9^ are obtained by equating the first two components of the 
third columns of that equation, thereby obtaining 

M 5 S 5 = (M6?’12 + Aeri3)c4 + {ne,r22 + \r2?,)si 
-t^5C5 = -A4(m 6?’12 + A6ri3)s4 + \i{ne,r22 + Aer23)c4 + M4(M6?'32 + Aer33) 
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which thus yield a unique value of for every value of Finally, with 
and known, it is a simple matter to calculate This is done upon 
solving for Qe from eq.(4.42a), i.e., 

Qe = 

and if the partitioning (4.12) of Qj is now recalled, a useful vector equation 
is derived, namely, 

P6 = Ql’Qlri (4.44) 

where ri is the first column of R. Let w denote the product Q)fri, i.e.. 



Hence, 



w = Qjri = 



rilCi + T21S4 

-A4(riiS4 - T21C4) + /44r3i 
- T21C4) + A4r3i 



Q^Qlri = 



W1C5 +W2S5 

A5(-WiS5 +W2C5) + W3/45 
/45(wiS5 - W2C5) + W3A5 
in which Wi denotes the *th component of w. Hence, and are deter- 
mined from the first two scalar equations of eq.(4.44), namely. 



ce — W1C5 + W2S5 

Sq = — W1A5S5 + W2A5C5 + W3/45 

thereby deriving a unique value of 9^ for every pair of values ( 84 , 9^). In 
summary, then, two values of 64 have been determined, each value deter- 
mining, in turn, one single corresponding set of 9^ and 9q values. Therefore, 
there are two sets of solutions for the orientation problem under study, 
which lead to two corresponding wrist postures. The two distinct postures 
of an orthogonal three-revolute spherical wrist for a given orientation of its 
EE are displayed in Fig. 4.18. 




FIGURE 4.18. The two configurations of a three-axis spherical wrist. 
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When combined with the four postures of a decoupled manipulator lead- 
ing to one and the same location of its wrist center — positioning problem — a 
maximum of eight possible combinations of joint angles for a single pose of 
the end-effector of a decoupled manipulator are found. 



4.5 Velocity Analysis of Serial Manipulators 

The relationships between the prescribed twist of the EE, also referred to as 
the Cartesian velocity of the manipulator, and the corresponding joint-rates 
are derived in this section. First, a serial n-axis manipulator containing 
only revolute pairs is considered. Then, relations associated with prismatic 
pairs are introduced, and finally, the joint rates of six-axis manipulators 
are calculated in terms of the EE twist. Particular attention is given to 
decoupled manipulators, for which simplified velocity relations are derived. 

We consider here the manipulator of Fig. 4.19, in which a joint coordinate 
9i, a joint rate 9i, and a unit vector e* are associated with each revolute 
axis. The Xj, Ij, Zi coordinate frame, attached to the (* — l)st link, is not 
shown, but its origin Oi is indicated. The relations that follow are apparent 
from that figure. 



Wo = 0 

wi = 9\ei 

W2= 0101+0262 (4.45) 

= 0161 + 0262 + • • • + 0n6« 




FIGURE 4.19. General n-axis manipulator. 
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and if the angular velocity of the EE is denoted by u>, then 

n 

UJ = UJri = OlQl + ^262 H h On^n = ^ 

1 



Likewise, from Fig. 4.19, one readily derives 



p = ai + a 2 4 h a„ 



(4.46) 



where p denotes the position vector of point P of the EE. Moreover, notice 
that all vectors of the above equation must be expressed in the same frame; 
otherwise, the addition would not be possible — vector a* was defined as 
expressed in the *th frame in eq.(4.3c). Upon differentiating both sides of 
eq.(4.46), we have 

p = ai + £12 4 h a„ (4.47) 



where 



dj = Wj X aj, * = 1 , 2 , . . . ,n 



(4.48) 



Furthermore, substitution of eqs.(4.45) and (4.48) into eq.(4.47) yields 



p = 6 »iei X ai 4 - (f'lei 4- 6*262) x a2 4 - 



: (4.49) 

+ ( 6^161 + ^262 + • • • + dnSn) X 

which can be readily rearranged as 

p = ^^i 6 i X (ai + a2 + • • • + a„) + O2B2 x (a2 + a3 + • • • + a„) 

A • • • A X a^ 

Now vector r* is defined as that joining Oi with P, directed from the 
former to the latter, i.e.. 



r® = + ^i+l + • • • + (4.50) 

and hence, p can be rewritten as 

n 

P = X 

1 

Further, let A and B denote the 3 x n matrices defined as 

A=[ei 62 ••• e„] (4.51a) 

B=[eixri 62 X T2 ••• 6„xr„] (4.51b) 

Furthermore, the n-dimensional joint-rate vector 6 is defined as 



T 
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Thus, (jj and p can be expressed in a more compact form as 

(jj = AO, p = B0 

the twist of the EE being defined, in turn, as 

t = ^ (4.52) 

P. 

The EE twist is thus related to the joint-rate vector 6 in the form 

J0 = t (4.53) 

where J is the Jacobian matrix, or Jacobian, for brevity, of the manipulator 
under study, first introduced by Whitney (1972). The Jacobian is defined 
as the 6 X n matrix shown below: 




It is important to note that if the axis of the *th revolute is denoted by 
TZi, then Jj is nothing but the Pliicker array of that line, with the moment 
of TZi being taken with respect to the operation point P of the EE. 

On the other hand, if the *th pair is not rotational, but prismatic, then the 
(* — l)st and the *th links have the same angular velocity, for a prismatic 
pair does not allow any relative rotation. However, vector a* joining the 
origins of the *th and (* + l)st frames is no longer of constant magnitude 
but undergoes a change of magnitude along the axis of the prismatic pair. 
That is, 

uJi = uJi-i, A = X aj + biBi 

One can readily prove, in this case, that 



+ • • • + + • • • + dn&n 

p = 0161 X ri + 0262 X T 2 4 h 0i_iej_i X rj_i + biBi 

+ ^^i+i6i+i X Tj+i + • • • + 0«e„ X a„ 
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from which it is apparent that the relation between the twist of the EE and 
the joint-rate vector is formally identical to that appearing in eq.(4.53) if 
vector 9 is defined as 



9=[9i 02 ■■■ 9i-i bi 9i+i ■■■ Onf" 



and the *th column of J changes to 

. [0 



(4.56) 



Note that the Pliicker array of the axis of the *th joint, if prismatic, is that 
of a line at infinity lying in a plane normal to the unit vector e*, as defined 
in eq.(3.35). 

In particular, for six-axis manipulators, J is a 6 x 6 matrix. Whenever 
this matrix is nonsingular, eq.(4.53) can be solved for 9, namely, 

9 = 3 h (4.57) 



Equation (4.57) is only symbolic, for the inverse of the Jacobian matrix 
need not be computed explicitly. Indeed, in the general case, matrix J can- 
not be inverted symbolically, and hence, 9 is computed using a numerical 
procedure, the most suitable one being the Gauss-elirnination algorithm, 
also known as EE decomposition (Golub and Van Loan, 1989). Gaussian 
elimination produces the solution by recognizing that a system of linear 
equations is most easily solved when it is in either upper- or lower-triangular 
form. To exploit this fact, matrix J is factored into the unique L and U 
factors in the form: 

J = LU (4.58a) 

where L is lower- and U is upper-triangular. Moreover, they have the forms 
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(4.58b) 



(4.58c) 



where in the particular case at hand, n = 6. Thus, the unknown vector of 
joint rates can now be computed from two triangular systems, namely. 



Ly = t, U6> = y (4.59) 

The latter equations are then solved, first for y and then for 9, by appli- 
cation of only forward and backward substitutions, respectively. The LU 
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decomposition of an n x n matrix requires multiplications and 
additions, whereas the forward substitution needed in solving the lower- 
triangular system of eq.(4.59) requires M" multiplications and addi- 
tions. Moreover, the backward substitution needed in solving the upper- 
triangular system of eq.(4.59) requires M”' multiplications and A!” addi- 
tions. These figures are (Dahlquist and Bjorck, 1974) 
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(4.60a) 

(4.60b) 

(4.60c) 



Thus, the solution of a system of n linear equations in n unknowns, using 
the LU-decomposition method, can be accomplished with M„ multiplica- 
tions and An additions, as given below (Dahlquist and Bjorck, 1974): 



M„ = ^(2Y + 9n + 1), A„ = + 3n - 4) (4.61a) 



Hence, the velocity resolution of a six-axis manipulator of arbitrary ar- 
chitecture requires Mq multiplications and A^ additions, as given below: 



Mg = 127, Mg = 100 



(4.61b) 



Decoupled manipulators allow an even simpler velocity resolution. For 
manipulators with this type of architecture, it is more convenient to deal 
with the velocity of the center C of the wrist than with that of the operation 
point P. Thus, one has 



where tc is defined as 



given by eqs.(3.84) and (3.85) as 



tc = 


je 




UJ 


tc = 


c 





using the twist-transfer formula 



tc 



1 O 
P - C 1 



tp 



with C and P defined as the cross-product matrices of the position vectors 
c and p, respectively. 

If in general, 3a denotes the Jacobian defined for a point A of the EE 
and Jp that defined for another point B, then the relation between 3a and 

3 b is 

3 b = V3a 



(4.62a) 
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where the 6x6 matrix U is defined as 



U = 



1 o 

A-B 1 



(4.62b) 



while A and B are now the cross-product matrices of the position vectors a 
and b of points A and B, respectively. Moreover, this matrix U is identical 
to the matrix defined under the same name in eq.(3.31), and hence, it 
belongs to the 6x6 unimodular group, i.e., the group of 6 x 6 matrices 
whose determinant is unity. Thus, 

det(Js) = det(JA) (4.63) 



We have then proven the result below: 

Theorem 4.5.1; The determinant of the Jacobian matrix of a six-axis 
manipulator is not affected under a change of operation point of the EE. 

Note, however, that the Jacobian matrix itself changes under a change 
of operation point. By analogy with the twist- and the wrench-transfer 
formulas, eq.(4.62a) can be called the Jacobian- transfer formula. 

Since C is on the last three joint axes, its velocity is not affected by the 
motion of the last three joints, and we can write 



c = 6 »iei X ri + 6202 x r2 + x ra 



where in the case of a decoupled manipulator, vector r* is defined as that 
directed from Oj to C. On the other hand, we have 

uj = 9 iei + 6262 + ^303 + ^464 + ^565 + 9 eee 



and thus, the Jacobian takes on the following simple form 



J 



Jii J12 

J21 o 



(4.64) 



where O denotes the 3x3 zero matrix, the other 3x3 blocks being given 
below, for manipulators with revolute pairs only, as 



11 = 


[ei 


62 


63] 


(4.65a) 


12 = 


[64 


65 


66 ] 


(4.65b) 


21 = 


[ei 


X ri 


62 X T2 63 X ra ] 


(4.65c) 



Further, vector 9 is partitioned accordingly: 



9 = 



0 , 
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where 





'01 




'0i' 


0a = 


02 


? — 


05 




03 




06 



Henceforth, the three components of 0a will be referred to as the arm rates, 
whereas those of 9^^ will be called the wrist rates. Now eqs.(4.53) can be 
written, for this particular case, as 

(4.66a) 

J21^a = c (4.66b) 

from which the solution is derived successively from the two systems of 
three equations and three unknowns that follow: 

J 2 i^a = c (4.67a) 

Ji2^«, = - Jii^a (4.67b) 

Prom the general expressions (4.60), then, it is apparent that each of 
the foregoing systems can be solved with the numbers of operations shown 
below: 

Ms = 23, Mg = 14 

Since the computation of the right-hand side of eq. (4.67b) requires, ad- 
ditionally, nine multiplications and nine additions, the total numbers of 
operations required to perform one joint-rate resolution of a decoupled ma- 
nipulator, My multiplications and Ay additions, are given by 

My = 55, Ay = 37 (4.68) 

which are fairly low figures and can be performed in a matter of microsec- 
onds using a modern processor. 

It is apparent from the foregoing kinematic relations that eq. (4.67a) 
should be first solved for 9a', with this value available, eq. (4.67b) can then 



be solved for 6y,. We thus have, symbolically, 

9a = J 21 C (4.69) 

9^=J^^\io-Jn0a) (4.70) 

Now, if we recall the concept of reciprocal bases introduced in Subsec- 
tion 2.7.1, the above inverses can be represented explicitly. Indeed, let 

A 21 = det(J 2 i) = (ei X ri) x (02 x r 2 ) • (eg x rg) (4.71) 

Ai2 = det(Ji2) = B4 X eg • eg (4.72) 



Then 



^21 



1 

A21 



[(eg X rg) X (eg X rg)]^ 
[(eg X rg) X (ei x ri)]^ 
i(ei X ri) X (eg x rg)]^ 



(4.73) 
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J 



-1 

12 



1 

Ai2 



(65 X eg)^ 
(eg X 64)^ 
(64 X 65)^ 



Therefore, 



and if we let 
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A21 



(62 X T2) X (63 X ra) • c 
(63 X T3) X (61 X ri) • c 
(61 X ri) X (62 X T2) • c 



VJ = U) - Jii6»a 



where va is read varpi, then 
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(4.74) 



(4.75a) 



(4.75b) 



(4.75c) 



4-5.1 Jacobian Evaluation 

The evaluation of the Jacobian matrix of a manipulator with n revolutes 
is discussed in this subsection, the presence of a prismatic pair leading 
to simplifications that will be outlined. Our aim here is to devise algo- 
rithms requiring a minimum number of operations, for these calculations 
are needed in real-time applications. We assume at the outset that all joint 
variables producing the desired EE pose are available. We divide this sub- 
section into two subsubsections, one for the evaluation of the upper part of 
the Jacobian matrix and one for the evaluation of its lower part. 



Evaluation of Submatrix A 

The upper part A of the Jacobian matrix is composed of the set { 6j }", and 
hence, our aim here is the calculation of these unit vectors. Note, moreover, 
that vector [oj ]i is nothing but the last column of Pj_i = Qi • • • Qi-i, our 
task then being the calculation of these matrix products. According to the 
DH nomenclature, 

[6i]i=[0 0 1]^ 

Hence, [6i]i is available at no cost. However, each of the remaining [6j]i 
vectors, for * = 2, . . . , n, is obtained as the last column of matrices Pj-i. 
The recursive calculation of these matrices is described below: 

Pi = Qi 
P2 = P1Q2 



P 



n 



— P n— 1 Qn 
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and hence, a simple algorithm follows: 

Pi ^ Qi 

For i = 2 to n do 

Pi ^ Pi-iQi 

enddo 

Now, since Pi is identical to Qi, the first product appearing in the do- 
loop, P1Q2, is identical to Q1Q2, whose two factors have a special struc- 
ture. The computation of this product, then, requires special treatment, 
which warrants further discussion because of its particular features. Prom 
the structure of matrices Qj, as displayed in eq.( 4 . 1 e), we have 

COS01 — Aisin^i yitisin^i cos<?2 — A2sin02 p,2sin02 

P2 = sin<?i Aicos^i — yiticos^i sin02 A2Cos<?2 — p,2cos02 

0 yiti Ai 0 p ,2 ^2 

The foregoing product is calculated now by first computing the prod- 
ucts A1A2, Aiyit2, and A2p,i, which involve only constant quantities, 

these terms thus being posture-independent. Thus, in tracking a prescribed 
Cartesian trajectory, the manipulator posture changes continuously, and 
hence, its joint variables also change. However, its DH parameters, those 
defining its architecture, remain constant. Therefore, the four above prod- 
ucts remain constant and are computed prior to tracking a trajectory, i.e., 
off-line. In computing these products, we store them as 

Ai 2 = A1A2, p -21 = Aip, 2 , p -12 = ^21 = A2P.1 

Next, we perform the on-line computations. First, let^ 



a -f- 


Ai t 


sin 


to 




r -f- 


sin 


01 


cos O2 




V -f- 


cos 


01 


cos O2 




u -f- 


cos 


01 


sin O2 


+ Ait 


V -f- 


sin 


01 


sin O2 


— Alt; 



and hence, 

w— (TsinUi — A2M + yiti2 sin p,2"« + A21 sin 

P2 = T + <TCOS 01 — A2U — p ,12 COS 01 11.2V — \21cosO1 

/iisin6»2 A21 cos 6»2 + 4t2i -4112 cos 6»2 + A12 

^Although V and v look similar, they should not be confused with each other, 
the former being the lowercase Greek letter upsilon. As a matter of fact, no 
confusion should arise, because upsilon is used only once, and does not appear 
further in the book. 
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As the reader can verify, the foregoing calculations consume 20 multiplica- 
tions and 10 additions. Now, we proceed to compute the remaining products 
in the foregoing do- loop. 

Here, notice that the product Pj_iQj, for 3 < * < n, can be computed 
recursively, as described below: Let Pj_i and Pj be given as 

Pll Pl2 Pl3 
Pj-l = P21 P22 P23 

_P31 P32 P33 _ 

\ P'll Pl2 Pl3 
P* = P2I P22 P23 
P?A P32 P33. 

Now matrix Pj is computed by first defining 

Ui = Pll sin 9i - pi 2 cos Oi 

Vi = P21 sin 0i - P22 cos 0i (4.76a) 

Wi = P31 sin 0i - P 32 cos 0i 

and 

p'll =piicos0i +Pi 2 sin 6 »i 
P'l2 = -ViXi +Pl3Pi 

p'l3 = UiPi +Pl3\i 

P21 =P2lCOS0i +P22sm0i 

P22 = -Vih +P23Pi (4.76b) 

P23 ^ ViPi -\- P23^i 
P31 = P31 COS 0i + P 32 sin 0i 
P32 = -WjAj +P33Pi 
P33 = V!iPi +P33\i 

Computing Ui, Vi, and Wj requires six multiplications and three addi- 
tions, whereas each of the pC entries requires two multiplications and one 
addition. Hence, the computation of each Pj matrix requires 24 multiplica- 
tions and 12 additions, the total number of operations required to compute 
the n — 2 products { Pj thus being 24(n — 2) + 20 = 24n — 28 multipli- 
cations and 12(n — 2) + 10 = 12n — 14 additions, for n > 2. Moreover, Pi, 
i.e., Qi, requires four multiplications and no additions, the total number of 
multiplications Ma and additions Aa required to compute matrix A thus 
being 

Ma = 24n - 24, Aa = 12n - 14 (4.77) 

Before concluding this section, a remark is in order: The reader may 
realize that P„ is nothing but Q, and hence, the same reader may wonder 
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whether we could not save some operations in the foregoing computations 
by stopping the above recursive algorithm at n — 1, rather than at n. This is 
not a good idea, for the above equality holds if and only if the manipulator 
is capable of tracking perfectly a given trajectory. However, reality is quite 
different, and errors are always present when tracking. As a matter of fact, 
the mismatch between P„ and Q is very useful in estimating orientation 
errors, which are then used in a feedback-control scheme to s}mthesize the 
corrective signals that are meant to correct those errors. 

Evaluation of Submatrix B 

The computation of submatrix B of the Jacobian is studied here. This 
submatrix comprises the set of vectors {oj x r* }". We thus proceed first 
to the computation of vectors r*, for * = 1, . . . , n, which is most efficiently 
done using a recursive scheme, similar to that of Horner for polynomial 
evaluation (Henrici, 1964), namely. 



[reje ^ [agje 

For i = 5 to 1 do 

[Yi]i ^ [aj]j + Qi[ri+i]i+i 

enddo 



In the foregoing algorithm, a simple scheme is introduced to perform the 
product Qj[rj+i]j+i, in order to economize operations: if we let 
[ri+i]i+i = [ri, T 2 , ra]^, then 



]ipl 



cos 9i 


— Aj sin 9i 


Pi sin 9i 




ri 


sin 9i 


Aj cos 9i 


— Pi cos 9i 




r2 


0 




Ai 







ri cos Oi — u sin Oi 
r\ sin 0i + u cos Oi 
r-2fJ-i + r-3\ 



(4.78a) 



where 

u = raAj - rspi (4.78b) 

Therefore, the product of matrix Qj by an arbitrary vector consumes eight 
multiplications and four additions. 

Furthermore, each vector [a* ]j, for * = 1, . . . , n, requires two multiplica- 
tions and no additions, as made apparent from their definitions in eq.(4.3b). 
Moreover, from the foregoing evaluation of Qi[rj+i ]i+i, it is apparent that 
each vector r*, in frame tFi, is computed with 10 multiplications and seven 
additions — two more multiplications are needed to calculate each vector 
[aj]j and three more additions are required to add the latter to vector 
Qi['”i+i ]i+i — the whole set of vectors {rj }" thus being computed, in tFi- 
coordinates, with 10(n — 1) + 2 = lOn — 8 multiplications and 7(n — 1) 
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additions, where one coordinate transformation, that of ri, is not counted, 
since this vector is computed directly in T\ . 

Now we turn to the transformation of the components of all the foregoing 
vectors into J^i-coordinates. First, note that we can proceed now in two 
ways: in the first, we transform the individual vectors e* and r* from Ti~ into 
J^i-coordinates and then compute their cross product; in the second, we first 
perform the cross products and then transform each of these products into 
J^i-coordinates. It is apparent that the second approach is more efficient, 
which is why we choose it here. 

In order to calculate the products e* x r* in J^j-coordinates, we let [ r* ] j = 
[pi, P 2 ^ Moreover, [ej]j = [0, 0, 1]^, and hence. 



[e* X Vi]i 



-P2 

Pi 

0 



which is thus obtained at no cost. Now, the transformation from Ti~ into 
J^i-coordinates is simply 



[oj X rj]i = X rj]j (4.79) 

In particular, [ei x ri]i needs no transformation, for its two factors are 
given in J^i-coordinates. The J^i-components of the remaining cross prod- 
ucts are computed using the general transformation of eq.(4.79). In the case 
at hand, this transformation requires, for each *, six multiplications and 
three additions, for this transformation involves the product of a full 3x3 
matrix, Pj-i, by a 3-dimensional vector, e* x r*, whose third component 
vanishes. Thus, the computation of matrix B requires Mb multiplications 
and Ab additions, as given below: 

Mb = lQn-U, AB = 10{n-l) (4.80) 

In total, then, the evaluation of the complete Jacobian requires Mj multi- 
plications and Aj additions, namely, 

Mj = 40n - 38, Aj = 22n - 24 (4.81) 

In particular, for a six-revolute manipulator, these figures are 202 multi- 
plications and 108 additions. 

Now, if the manipulator contains some prismatic pairs, the foregoing 
figures diminish correspondingly. Indeed, if the *th joint is prismatic, then 
the *th column of the Jacobian matrix changes as indicated in eq.(4.56). 
Hence, one cross-product calculation is spared, along with the associated 
coordinate transformation. As a matter of fact, as we saw above, the cross 
product is computed at no cost in local coordinates, and so each prismatic 
pair of the manipulator reduces the foregoing numbers of operations by 
only one coordinate transformation, i.e., by 10 multiplications and seven 
additions. 
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4-5.2 Singularity Analysis of Decoupled Manipulators 

In performing the computation of the joint rates for a decoupled manipu- 
lator, it was assumed that neither J 12 nor J 21 is singular. If the latter is 
singular, then none of the joint rates can be evaluated, even if the former is 
nonsingular. However, if J 21 is nonsingular, then eq.(4.66a) can be solved 
for the arm rates even if J 12 is singular. Each of these sub-Jacobians is 
analyzed for singularities below. 

We will start analyzing J21, whose singularity determines whether any 
joint-rate resolution is possible at all. First, we note from eq. (4.65c) that 
the columns of J 21 are the three vectors ei x ri, 62 x r 2 , and 63 x ra. 
Hence, J 21 becomes singular if either these three vectors become coplanar 
or at least one of them vanishes. Furthermore, neither the relative layout 
of these three vectors nor their magnitudes change if the manipulator un- 
dergoes a motion about the first revolute axis while keeping the second 
and the third revolute axes locked. This means that 9i does not affect the 
singularity of the manipulator, a result that can also be derived from in- 
variance arguments — see Section 2.6 — and by noticing that singularity is, 
indeed, an invariant property. Hence, whether a configuration is singular or 
not is independent of the viewpoint of the observer, a change in 9i being 
nothing but a change of viewpoint. 

The singularity of a three- re volute arm for positioning tasks was analyzed 
by Burdick (1995), by recognizing that (*) given three arbitrary lines in 
space, the three revolute axes in our case, it is always possible to find a 
set of lines that intersects all three, and (n) the moments of the three 
lines about any point on the intersecting line are all zero. As a matter 
of fact, the locus of those lines is a quadric ruled surface, namely, a one- 
sheet hyperboloid — see Exercise 3.4. Therefore, if the endpoint of the third 
moving link lies in this quadric, the manipulator is in a singular posture, 
and velocities of C along the intersecting line cannot be produced. This 
means that the manipulator has lost, to a first order, one degree of freedom. 
Here we emphasize that this loss is meaningful only at a first order because, 
in fact, a motion along that intersecting line may still be possible, provided 
that the full nonlinear relations of eq.(4.16) are considered. If such a motion 
is at all possible, however, then it is so only in one direction, as we shall see 
in Case 2 below. Motions in the opposite direction are not feasible because 
of the rigidity of the links. 

We will illustrate the foregoing concepts as they pertain to the most 
common types of industrial manipulators, i.e., those of the orthogonal type. 
In these cases, two consecutive axes either intersect at right angles or are 
parallel; most of the time, the first two axes intersect at right angles and 
the last two are parallel. Below we study each of these cases separately. 



Case 1; Two consecutive axes intersect and C lies in their plane. 
Here, the ruled hyperboloid containing the lines that intersect all 
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three axes degenerates into a plane, namely, that of the two inter- 
secting axes. For conciseness, let us assume that the first two axes 
intersect, but the derivations are the same if the intersecting axes are 
the last two. Moreover, let 0\2 be the intersection of the first two 
axes, IIi2 being the plane of these axes and rii2 its normal. If we re- 
call the notation adopted in Section 4 . 5 , we have now that the vector 
directed from O12 to C can be regarded as both ri and r2. Further- 
more, 6i X ri and 62 x r2 (= 62 x ri) are both parallel to rii2. Hence, 
the first two axes can only produce velocities of C in the direction of 
rii2. As a consequence, velocities of C in Hi2 and perpendicular to 
63 X ra cannot be produced in the presence of this singularity. The set 
of infeasible velocities, then, lies in a line normal to rii2 and 63 x ra, 
whose direction is the geometric representation of the nullspace of . 
Likewise, the manipulator can withstand forces applied at C in the 
direction of the same line purely by reaction wrenches, i.e., without 
any motor torques. The last issue falls into the realm of manipulator 
statics, upon which we will elaborate in Section 4 . 7 . 

We illustrate this singularity, termed here shoulder singularity, in a 
manipulator with the architecture of Fig. 4 . 3 , as postured in Fig. 4 . 20 . 
In this figure, the line intersecting all three arm axes is not as obvious 
and needs further explanation. This line is indicated by C in that 
figure, and is parallel to the second and third axes. It is apparent that 
this line intersects the first axis at right angles at a point /. Now, if 
we take into account that all parallel lines intersect at infinity, then 
it becomes apparent that C intersects the axis of the third revolute 
as well, and hence, C intersects all three axes. 

Case 2; Two consecutive axes are parallel and C lies in their plane, 
as shown in Fig. 4 . 21 . For conciseness, again, we assume that the 
parallel axes are now the last two, a rather common case in com- 
mercial manipulators, but the derivations below are the same if the 
parallel axes are the first two. We now let H23 be the plane of the 
last two axes and n23 its normal. Furthermore, 63 = 62, r2 = ri, and 
62 X T3 = 0(62 X T2), where 

^ ^ H 

in terms of the Denavit-Hartenberg notation, thereby making appar- 
ent that the last two columns of J21 are linearly dependent. Moreover, 
62 X T2 and, consequently, 63 x r3 are parallel to n23, the last two 
axes being capable of producing velocities of C only in the direction 
of n23. Hence, velocities of C in H23 that are normal to ei x ri, 
i.e., along line £, cannot be produced in this configuration, and the 
manipulator loses, again, to a first-order approximation, one degree 
of freedom. The set of infeasible velocities, then, is parallel to the 
line C of Fig. 4 . 21 , whose direction is the geometric representation of 
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FIGURE 4.20. Shoulder singularity of the Puma robot. 

the nullspace of The singularity displayed in the foregoing fig- 
ure, termed here the elbow singularity, pertains also to a manipulator 
with the architecture of Fig. 4.3. Notice that motions along C in the 
posture displayed in Fig. 4.21 are possible, but only in one direction, 
from C to O 2 • 

With regard to the wrist singularities, these were already studied when 
solving the orientation problem for the inverse kinematics of decoupled 
manipulators. Here, we study the same in light of the sub-Jacobian J 12 
of eq.(4.65b). This sub-Jacobian obviously vanishes when the wrist is so 
configured that its three revolute axes are coplanar, which thus leads to 

64 X 65 • 66 = 0 

Note that when studying the orientation problem of decoupled manipu- 
lators, we found that orthogonal wrists are singular when the sixth and 
fourth axes are aligned, in full agreement with the foregoing condition. In- 
deed, if these two axes are aligned, then 64 = — 65 , and the above equation 
holds. 



4.5.3 Manipulator Workspace 

The workspace of spherical wrists for orientation tasks was discussed in 
Subsection 4.4.2. Here we focus on the workspaces of three-axis positioning 
manipulators in light of their singularities. 
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FIGURE 4.21. Elbow singularity of the Puma robot. 




EIGURE 4.22. Workspace of a Puma manipulator (a) top view; (b) side view; 
(c) perspective. 
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In order to gain insight into the problem, we study first the workspace of 
manipulators with the architecture of Fig. 4.3. Figures 4.20 and 4.21 show 
such a manipulator with point C at the limit of its positioning capabilities 
in one direction, i.e., at the boundary of its workspace. Moreover, with re- 
gard to the posture of Fig. 4.20, it is apparent that the first singularity is 
preserved if (*) point C moves on a line parallel to the first axis and inter- 
secting the second axis; and (m) with the second and third joints locked, 
the first joint goes through a full turn. Under the second motion, the line 
of the first motion sweeps a circular cylinder whose axis is the first manip- 
ulator axis and with radius equal to 63, the shoulder offset. This cylinder 
constitutes a part of the workspace boundary, the other part consisting of 
a spherical surface. Indeed, the second singularity is preserved if (*) with 
point C in the plane of the second and third axes, the second joint makes a 
full turn, thereby tracing a circle with center on £ 2 , a distance 63 from the 
first axis, and radius r = a ,2 + + 64 ; and (m) with point C still in the 

plane of the second and third joints, the first joint makes a full turn. Un- 
der the second motion, the circle generated by the first motion describes 
a sphere of radius R = + because any point of that circle lies a 

distance R from the intersection of the first two axes. This point thus be- 
comes the center of the sphere, which is the second part of the workspace, 
as shown in Fig. 4.22. 

The determination of the workspace boundaries of more general manip- 
ulators requires, obviously, more general approaches, like that proposed 
by Ceccarelli (1996). By means of an alternative approach, Ranjbaran 
et al. (1992) found the workspace boundary with the aid of the general 
characteristic equation of a three-revolute manipulator. This equation is a 
quartic polynomial, as displayed in eq.(4.25). From the discussion of Sub- 
section 4.4.1, it is apparent that at singularities, two distinct roots of the 
IKP merge into a single one. This happens at points where the plot of the 
characteristic polynomial of eq.(4.25) is tangent to the T3 axis, which occurs 
in turn at points where the derivative of this polynomial with respect to T3 
vanishes. The condition for 0^, to correspond to a point C on the boundary 
of the workspace is, then, that both the characteristic polynomial and its 
derivative with respect to T3 vanish concurrently. These two polynomials 
are displayed below: 

P(t 3 ) = R4 + StI + Tt| + Ut 3 + V = 0 (4.82a) 

= 4i?r| + + 2 Tt 3 + U = 0 (4.82b) 

with coefficients i?. S', T, U, and V defined in eqs.(4.26a-e). From these 
equations and eqs.(4.19d-f) and (4.20d-f), it is apparent that the foregoing 
coefficients are solely functions of the manipulator architecture and the 
Cartesian coordinates of point C. Moreover, from the same equations, it 
is clear that the above coefficients are all quadratic in and 

quartic in zc- Thus, since the Cartesian coordinates xc and yc do not 
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appear in the foregoing coefficients explicitly, the workspace is symmetric 
about the Z\ axis, a result to be expected by virtue of the independence 
of singularities from angle d\. Hence, the workspace boundary is given 
by a function /(p^, zc) = 0 that can be derived by eliminating T 3 from 
eqs. (4.82a & b). This can be readily done by resorting to any elimination 
procedure, the simplest one being dialytic elimination, as discussed below. 

In order to eliminate T3 from the above two equations, we proceed in 
two steps: In the first step, six additional polynomial equations are derived 
from eqs. (4. 82a & b) by multiplying the two sides of each of these equations 
by T 3 , t|, and Tg , thereby obtaining a total of eight polynomial equations 
in Tg, namely, 

RtI + S't| + Tt| + U4 +VtI =0 
4i?r| + 3S't| + 2Tt^ + [/ t | = 0 
RtI + StI + TtI + UtI + VtI = 0 
4i?r| + UStI + 2Tt| + [/ t | = 0 
RtI + StI + TtI + UtI + Vrg = 0 
4i?Tg + 3 S't| + 2TtI + Ut3 =0 
RtI + StI + TtI + UT3 + V = 0 
4i?r| + 3 S't| + 2Tt3 -\- U = 0 

In the second elimination step we write the above eight equations in linear 
homoqeneous form, namely, 

Mrg = 0 (4.83a) 

with the 8 x 8 matrix M and the 8 -dimensional vector Tg defined as 

-R S T U V 0 0 0 - 

0 4R :iS 2T U 0 0 0 

ORSTUVOO 
^0 0 4R :iS 2T U 0 0 

“ 0 0 R S T U V 0 ’ 

0 0 0 4i? 35' 2T [/ 0 

00 ORSTUV 
.0 0 0 0 4R :iS 2T U. 

It is now apparent that any feasible solution of eq.(4.83a) must be nontriv- 
ial, and hence, M must be singular. The desired boundary equation is then 
derived from the singularity condition on M, i.e., 

/(p^, Zc) = det(M) = 0 (4.84) 

Note that all entries of matrix M are linear in the coefficients R, S, . . ., 
V, which are, in turn, quadratic in p^ and quartic in zc- Therefore, the 
workspace boundary is a surface of 16th degree in p^ and of 32nd degree 
in Zc. 

We used the foregoing procedure, with the help of symbolic computa- 
tions, to obtain a rendering of the workspace boundary of the manipulator 
of Figs. 4.13-4.15, the workspace thus obtained being displayed in Fig. 4.23. 
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Regions Number of Solutions 




FIGURE 4.23. The workspace of the manipulator of Figs. 4. 17-19. 

4.6 Acceleration Analysis of Serial Manipulators 

The subject of this section is the computation of vector 9 of second joint- 
variable derivatives, also called the joint accelerations. This vector is com- 
puted from Cartesian position, velocity, and acceleration data. To this end, 
both sides of eq.(4.53) are differentiated with respect to time, thus obtain- 
ing 

J0 = t - (4.85) 

and hence, 

'e = 3-\i-39) (4.86) 

Prom eq.(4.85), it is clear that the joint-acceleration vector is computed in 
exactly the same way as the joint-rate vector. In fact, the LU decomposition 
of J is the same in this case and hence, need not be recomputed. All that is 
needed is the solution of a lower- and an upper-triangular system, namely, 

Lz = t — 39, U9 = z 

The two foregoing systems are solved first for z and then for 9 by forward 
and backward substitution, respectively. The first of the foregoing systems 
is solved with M" multiplications and A" additions; the second with M”' 
multiplications and A!” additions. These figures appear in eqs.(4.62b & c). 
Thus, the total numbers of multiplications, Mt, and additions. At, that the 
forward and backward solutions of the aforementioned systems require are 

Mt = n^. At = n{n — 1) (4.87) 

In eq.(4.85), the right-hand side comprises two terms, the first being the 
specified time-rate of change of the twist of the EE, or twist-rate, for brevity. 
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which is readily available. The second term is not available and must be 
computed. This term involves the product of the time-derivative of J times 
the previously computed joint-rate vector. Hence, in order to evaluate the 
right-hand side of that equation, all that is further required is J. Prom 
eq.(4.54a), one has 




where, from eqs.(4.51a & b). 



A = [ei 62 • • 


• e„] 


(4.88a) 


B = [ui U 2 


• • Un] 


(4.88b) 


and Uj denotes 6j x r*, for * = 1, 2, . . . , n. 


Moreover, 




6i = tUo X 6i = 0 




(4.89a) 


6j = X 6j = Wj X 6j, 


* = 2, 3, . . . , n 


(4.89b) 


and 


Uj = 6j X Tj +6j X l-j. 


i = 1, 2, . . . , n 


(4.89c) 



Next, an expression for r* is derived by time-differentiating both sides of 
eq.(4.50), which produces 



r® — + ^i+i + • • • + d„, * — 1, 2, ... n 

Recalling eq.(4.48), the above equation reduces to 

f-j = Wj X aj + Wj+i X aj+i H h x a„ (4.90) 

Substitution of eqs.(4.89) and (4.90) into eqs.(4.88a & b) leads to 
A=[0 X 62 ••• w„_ixe„] 

B = [eixri Wi 2 xr 2 + 62 X 1-2 ••• x r„ + 6 „ x r„ ] 

with r-fc and ujk,k+i defined as 

n 

rfc = ^WjXaj, k = l,...,n (4.91a) 

k 

^Ok,k+l = <^k X ek+i, k = l,...,n-l (4.91b) 

The foregoing expressions are invariant and hence, valid in any coordinate 
frame. However, they are going to be incorporated into matrix J, and then 
the latter is to be multiplied by vector 6, as indicated in eq.(4.85). Thus, 
eventually all columns of both A and B will have to be represented in the 
same coordinate frame. Hence, coordinate transformations will have to be 
introduced in the foregoing matrix columns in order to have all of these 
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represented in the same coordinate frame, say, the first one. We then have 
the expansion below: 



30 = 


■ 0 ■ 


+ O 2 


&2 


4 - ■ ■ ■ On 






Ul 




U2 




Un 



The right-hand side of eq.(4.92) is computed recursively as described below 
in five steps, the number of operations required being included at the end 
of each step. 

1. Compute 

[wi]i ^ 0i[ei]i 

For i = 1 to n — 1 do 

]i+l + 

enddo 8(n — 1) M & 5(n — 1) A 

2. Compute { [dj ]j }": 

[ei]i ^ [0]i 

For i = 2 to n do 

[ei]i ^ [wj X Bi]i 



enddo OM & OA 

3. Compute { [rj]j jj: 

[ tn ]n ^ ^ ]n 

For i = n — 1 to 1 do 

[Yi]i ^ [wj X aj]j + Qj[rj+i]j+i 

enddo (14n — 8)M & (lOn — 7)A 

4. Compute { [uj]j }" using the expression appearing in eq.(4.89c): 

[ui ]i ^ [ei X ri ]i For i = 2 to n do 

[Uj]j ^ [dj X Tj +6j X Yi]i 



enddo 



4(n — 1) M & 3(n — 1) A 
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5. Compute 39: 

Let V = 39, which is a 6-dimensional vector. A coordinate trans- 
formation of its two 3-dimensional vector components will be imple- 
mented using the 6x6 matrices Uj, which are defined as 



Ui 



Qi O 
O Qj 



where O stands for the 3x3 zero matrix. Thus, the foregoing 6x6 ma- 
trices are block-diagonal, their diagonal blocks being simply matrices 
Qj. One then has the algorithm below: 



■ 

For i 



J n 

n — 1 to 1 do 



Uj[v]i+i 



enddo 

j0^[v]i 21(n-l) + 4M& 13(n-l)A 



thereby completing the computation of 39. 



The figures given above for the floating-point operations involved were 
obtained based on a few facts, namely, 

1. It is recalled that [ejjj = [0, 0, 1]^. Moreover, if we let [w]j = 
[wx, Wy, WzY be an arbitrary 3-dimensional vector, then 



[e* X w]i 



-Wy 

Wx 

0 



this product thus requiring zero multiplications and zero additions. 

2. [ej]i, computed as in eq.(4.89b), takes on the form [ujy, —lOx, 0]^, 
where lOx and lOy are the Xj and Ij components of tUj. Moreover, let 
[rj]i = [x, y, zY . Then 



[dj X rj]i 



— ZUJx 

— ZLUy 

XLVx + yu)y 



and this product is computed with four multiplications and one ad- 
dition. 
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3. As found in Subsection 4.5.1, any coordinate transformation from Ti 
to or vice versa, of any 3-dimensional vector is computed with 

eight multiplications and four additions. 

Thus, the total numbers of multiplications and additions required to 
compute J0 in frame T\, denoted by Mj and Aj, respectively, are as 
shown below: 

Mj = 47n - 37, Aj = Sin - 28 

Since the right-hand side of eq.(4.85) involves the algebraic sum of two 
6-dimensional vectors, then, the total numbers of multiplications and ad- 
ditions needed to compute the aforementioned right-hand side, denoted by 
Mr and A^, are 

Mr = 47n - 37, Ar = 31n - 22 

These figures yield 245 multiplications and 164 additions for a six-revolute 
manipulator of arbitrary architecture. Finally, if the latter figures are added 
to those of eq.(4.87), one obtains the numbers of multiplications and addi- 
tions required for an acceleration resolution of a six-revolute manipulator 
of arbitrary architecture as 

Ma = 281, A„ = 194 

Furthermore, for six-axis, decoupled manipulators, the operation counts 
of steps 1 and 2 above do not change. Flowever, step 3 is reduced by 42 
multiplications and 30 additions, whereas step 4 by 12 multiplications and 
9 additions. Moreover, step 5 is reduced by 63 multiplications and 39 addi- 
tions. With regard to the solution of eq.(4.85) for 0, an additional reduction 
of floating-point operations, or flops, is obtained, for now we need only 18 
multiplications and 12 additions to solve two systems of three equations 
with three unknowns, thereby saving 18 multiplications and 18 additions. 
Thus, the corresponding figures for such a manipulator, M' and A^, re- 
spectively, are 

M'=146, a; = 98 



4.7 Static Analysis of Serial Manipulators 

In this section, the static analysis of a serial n-axis manipulator is under- 
taken, particular attention being given to six-axis, decoupled manipulators. 
Let Ti be the torque acting at the *th revolute or the force acting at the *th 
prismatic pair. Moreover, let r be the n-dimensional vector of joint forces 
and torques, whose *th component is Tj, whereas w = [n^, denotes 

the wrench acting on the EE, with n denoting the resultant moment and f 
the resultant force applied at point P of the end-effector of the manipulator 
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of Fig. 4.19. Then the power exerted on the manipulator by all forces and 
moments acting on the end-effector is 

n^; = W^t = + f^p 

whereas the power exerted on the manipulator by all joint motors, IIj, is 

Uj = t'^6 (4.93) 

Under static, conservative conditions, there is neither power dissipation 
nor change in the kinetic energy of the manipulator, and hence, the two 
foregoing powers are equal, which is just a restatement of the First Law of 
Thermodynamics or equivalently, the Principle of Virtual Work, i.e., 

w^t = t’^6 (4.94a) 

Upon substitution of eq.(4.53) into eq.(4.94a), we obtain 

= t'^6 (4.94b) 

which is a relation valid for arbitrary 6 . Under these conditions, if J is not 
singular, eq. (4.94b) leads to 

J^w = T (4.95) 

This equation relates the wrench acting on the EE with the joint forces and 
torques exerted by the actuators. Therefore, this equation finds applications 
in the sensing of the wrench w acting on the EE by means of torque sensors 
located at the revolute axes. These sensors measure the motor-supplied 
torques via the current flowing through the motor armatures, the sensor 
readouts being the joint torques — or forces, in the case of prismatic joints — 
{ Tfc }", grouped into vector r. 

For a six-axis manipulator, in the absence of singularities, the foregoing 
equation can be readily solved for w in the form 

w = 



where stands for the inverse of J^. Thus, using the figures recorded 
in eq. (4.61b), w can be computed from eq.(4.95) with 127 multiplications 
and 100 additions for a manipulator of arbitrary architecture. Flowever, 
if the manipulator is of the decoupled type, the Jacobian takes on the 
form appearing in eq.(4.64), and hence, the foregoing computation can be 
performed in two steps, namely. 



1 12'^w 



- Jnn^^ 



where is the resultant moment acting on the end-effector when f is 
applied at the center of the wrist, while r has been partitioned as 



Ta 

w 



T = 
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with Ta and defined as the wrist and the arm torques, respectively. 
These two vectors are given, in turn, as 





n 






Ta = 


T2 


? w 


T5 











Hence, the foregoing calculations, as pertaining to a six-axis, decoupled 
manipulator, are performed with 55 multiplications and 37 additions, which 
follows from a result that was derived in Section 4.5 and is summarized in 
eq.(4.68). 

In solving for the wrench acting on the EE from the above relations, the 
wrist equilibrium equation is first solved for n^^,, thus obtaining 

nw=^i2'Tw (4.96) 

where 3^2 stands for the inverse of J^f 2 , and is available in eq.(4.74). There- 
fore, 



= -r — [ (65 X ee) (ee x 64) (64 x 65) ] Ty , 

2X12 

= -r — [7-4(65 X 65) +T5(66 X 64) + Te(64 X 65)] (4.97) 

2 x 12 

Now, if we let 

Ta^Ta-^l-^ny, (4.98) 

we have, from eq.(4.73). 



f = [ U2 X U3 U3 X Ui 



Ui X U2 



Tg 

A 21 



where 



Uj = 6j X Yi 



or 

f = [ti(u2 X U3) +T2(u3 X Ui) + T3 (ui X U2) ] ( 4 . 99 ) 

2X21 

4.8 Planar Manipulators 

Shown in Fig. 4.24 is a three-axis planar manipulator. Note that in this 
case, the DH parameters hi and Oj vanish, for * = 1 , 2 , 3, the nonvanishing 
parameters a* being indicated in the same figure. Below we proceed with the 
displacement, velocity, acceleration, and static analyses of this manipulator. 
Here, we recall a few relations of planar mechanics that will be found useful 
in the discussion below. 
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A 2 X 2 matrix A can be partitioned either columnwise or rowwise, as 
shown below: 



A = [a b] 




where a, b, c, and d are all 2-dimensional column vectors. Furthermore, 
let E be defined as an orthogonal matrix rotating 2-dimensional vectors 
through an angle of 90° counterclockwise. Hence, 



We thus have 

Fact 4.8.1 

= E^ = -E 

and hence. 

Fact 4.8.2 



(4.100) 



E^ = -1 



where 1 is the 2x2 identity matrix. 
Moreover, 

Fact 4.8.3 



det(A) = -a^Eb = b^Ea = -c^Ed = d^Ec 

and 

Fact 4.8.4 



det(A) 



—a" 



E 



det(A) 



E[-d 



4.8.1 Displacement Analysis 

The inverse kinematics of the manipulator at hand now consists of deter- 
mining the values of angles 0j, for * = 1, 2, 3, that will place the end-effector 
so that its operation point P will be positioned at the prescribed Carte- 
sian coordinates x, y and be oriented at a given angle (f> with the X axis 
of Fig. 4.24. Note that this manipulator can be considered as decoupled, 
for the end-effector can be placed at the desired pose by first positioning 
point O3 with the aid of the first two joints and then orienting it with the 
third joint only. We then solve for the joint angles in two steps, one for 
positioning and one for orienting. 

We now have, from the geometry of Fig. 4.24, 

aici + a2Ci2 = X 

aisi + CI2S12 = y 
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FIGURE 4.24. Three-axis planar manipulator. 

where x and y denote the Cartesian coordinates of point O3, while C12 and 
S12 stand for cos(0i+02) and sin(0i+02), respectively. We have thus derived 
two equations for the two unknown angles, from which we can determine 
these angles in various ways. For example, we can solve the problem using 
a semigraphical approach similar to that of Subsection 8 . 2 . 2 . 

Indeed, from the two foregoing equations we can eliminate both C12 and 
S12 by solving for the second terms of the left-hand sides of those equations, 
namely, 

c*2Ci2 = X — a\Ci ( 4 . 101 a) 

a2«i2 = y - aui ( 4 . 101 b) 



If both sides of the above two equations are now squared, then added. 





FIGURE 4.25. The two real solutions of a planar manipulator. 
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FIGURE 4.26. The two real values of 6i depicted in Fig. 4.25. 

and the ensuing sum is equated to a^, we obtain, after simplification, a 
linear equation in ci and si that represents a line C in the ci-si plane: 

£: —of -\- 0,2 + 2aixci + 2aiysi — = 0 (4.102) 

Clearly, the two foregoing variables are constrained by a quadratic equation 
defining a circle C in the same plane: 

C: cl + sl = l 

which is a circle C of unit radius centered at the origin of the aforementioned 
plane. The real roots of interest are then obtained as the intersections of C 
and C. Thus, the problem can admit (*) two real and distinct roots, if the 
line and the circle intersect; (m) one repeated root if the line is tangent to 
the circle; and (in) no real root if the line does not intersect the circle. 

With Cl and si known, angle 0\ is fully determined. Note that the two 
real intersections of C with C provide each one value of <?i, as depicted in 
Fig. 4.26. 

Once 0\ and O 2 are available, is readily derived from the geometry of 
Fig. 4.24, namely, 

^3 = <(> - (^1 + ^2) 

and hence, each pair of (<?i, O 2 ) values yields one single value for O^,. Since 
we have two such pairs, the problem admits two real solutions. 

4.8.2 Velocity Analy,sis 

Velocity analysis is most easily accomplished if the general velocity relations 
derived in Section 4.5 are recalled and adapted to planar manipulators. 
Thus we have, as in eq.(4.53). 



J6> = t 



(4.103a) 
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where now, 









'h' 






ei 62 


63 


, 0 = 




, t = 




6i X ri 62 X Y2 


63 X T3 




f\ 




_p_ 








_^ 3 _ 







(4.103b) 



and {rj are defined as in eq.(4.50), i.e., as the vectors directed from Oj 
to P. As in the previous subsection, we assume here that the manipulator 
moves in the X-Y plane, and hence, all revolute axes are parallel to the Z 
axis, vectors e* and r*, for * = 1, 2, 3, thus taking on the forms 





'O' 




Xi 


6l = 62 = 63 = 6 = 


0 


, Ti = 


Vi 




1 




0 



with t reducing to 

t=[0 0 (j) ip yp 0]^ (4.103c) 



in which xp and yp denote the components of the velocity of P . Thus, 



6j X Yi 



-Vi 

Xi 

0 



and hence, the foregoing cross product can be expressed as 



6j X Yi 



Esj 

0 



where E was defined in eq.(4.100) and Sj is the 2-dimensional projection 
of Yi onto the x-y plane of motion, i.e., Sj = [xj yi] . Equation (4.103a) 
thus reduces to 

- 0 
1 

Es 
. 0 

where 0 is the 2-dimensional zero vector and p is now reduced to p = 
[i, yY ■ In summary, then, by working only with the three nontrivial equa- 
tions of eq.(4.104), we can represent the velocity relation using a 3 x 3 
Jacobian in eq.(4.103a). To this end, we redefine J and t as 



0 


0 - 




- 0 - 


to 


1 

Es3 


e = 


4> 

p 


0 


0 . 




. 0 . 



(4.104) 



J = 


1 


1 


1 


t = 






Esi 


Es2 


Es3_ 




_P_ 



(4.105) 



The velocity resolution of this manipulator thus reduces to solving for the 
three joint rates from eq.(4.103a), with J and t defined as in eq.(4.105), 
which thus leads to the system below: 



1 

Esi 


to 


1 

Es3 




1 

(M 

1 


= 


_P_ 








L'^sj 







(4.106) 
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Solving for { Oi is readily done by first reducing the system of equa- 
tions appearing in eq.(4.103a) to one of two equations in two unknowns 
by resorting to Gaussian elimination. Indeed, if the first scalar equation 
of eq.(4.106) is multiplied by Esi and the product is subtracted from the 
2-dimensional vector equation, we obtain 

'll 1 

_0 E(s2-si) E(s3-Si) 

from which a reduced system of two equations in two unknowns is readily 
obtained, namely. 



4>. 

p — <^Esi 



(4.107) 



[E(s2-si) E(s3-si)] 



p — ())Esi 



(4.108) 



The system of equations (4.108) can be readily solved if Fact 4.8.4 is 
recalled, namely. 



E(p — ())Esi 



'e2~ 


1 


-(S3 -Si)^E 


A. 


^ A 


(S2 -Sl)^E _ 




1 


(S3 -Si)^(p - 




^ A 


_-(s2 -Si)^(p 



where A is the determinant of the 2x2 matrix involved, i.e., 

A = det([E(s 2 - si) E(s 3 -si)]) = -(S 2 -si)^E(s 3 -si) (4.109) 
We thus have 



(S3 - Si)^(p - (^Esi) 
(S2 -Si)^E(s3 - Si) 

(S2 - Si)^(p - (^Esi) 
(S2 - Si)^E(s 3 - Si) 



(4.110a) 

(4.110b) 



Further, 0\ is computed from the first scalar equation of eq.(4.106), i.e., 

0i = ^-{02 + 0z) (4.110c) 

thereby completing the velocity analysis. 

The foregoing calculations are summarized below in algorithmic form, 
with the numbers of multiplications and additions indicated at each stage. 
In those numbers, we have taken into account that a multiplication of E by 
any 2-dimensional vector incurs no computational cost, but rather a simple 
rearrangement of the entries of this vector, with a reversal of one sign. 



21 



1. d. 



S2 - Si 



0M + 2M 
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2 - d3i 


^ S 3 - Si 


OM + 2A 


3. A e- 


- d)()Ed 2 i 


2M + lA 


4. 


■ p — ())Esi 


2M + 2A 


5. u ^ 


■ u/A 


2M + 0A 


6 . 02 e- 


- U^d 3 i 


2M + lA 


7. 03^ 


- -U^d 21 


2M + lA 


8 . 01 e- 


- 4> -02 -03 


0M + 2A 



The complete calculation of joint rates thus consumes only lOM and 11 A, 
which represents a savings of about 67% of the computations involved if 
Gaussian elimination is applied without regarding the algebraic structure 
of the Jacobian J and its kinematic and geometric significance. In fact, 
the solution of an arbitrary system of three equations in three unknowns 
requires, from eq. (4.61a), 28 additions and 23 multiplications. If the cost 
of calculating the right-hand side is added, namely, 4A and 6 M, a total 
of 32A and 29M is required to solve for the joint rates if straightforward 
Gaussian elimination is used. 

4-8.3 Acceleration Analysis 

The calculation of the joint accelerations needed to produce a given twist 
rate of the EE is readily accomplished by differentiating both sides of 
eq.(4.103a), with definitions (4.105), i.e., 

39 + 39 = i 

from which we readily derive a system of equations similar to eq.(4.103a) 
with 9 as unknown, namely, 

39 = i-39 

where 



0 

Esi 


0 

Es2 


0 

Es3_ 


III 


1 

i.: Qi.: Qi.: 

to 

1 


III 


_p_ 










L'^sj 


1 





S 3 = (^^1 + ^2 + <?3)Ea3 

S2 = »2 + S3 = (di + ^2)Ea2 + S3 

Si = ai + S2 = 0iEsi + S2 



and 
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Now we can proceed by Gaussian elimination to solve for the joint accelera- 
tions in exactly the same manner as in Subsection 4.8.2, thereby obtaining 
the counterpart of eq.(4.108), namely, 



[E(s 2 -Si) E(s 3 -Si)] 

with w defined as 



w 



W = p - E(6»iSi + 02^2 + 6»3S3 + 4>^i) 
and hence, similar to eqs.(4.110a-c), one has 

(S3 -Si)^W 

A 

(S2 -Si)^W 

A 

( j ) -{02 + 03 ) 



O2 = 

h = 
01 = 



(4.111a) 

(4.111b) 

(4.112a) 

(4.112b) 

(4.112c) 



Below we summarize the foregoing calculations in algorithmic form, 
dicating the numbers of operations required at each stage. 



1. S 3 4 - 


- {01 + 02 + ^^a)Ea3 


2M & 


2. S 2 4 - 


- {0\ + <i 2 )Ea 2 + S 3 
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where d 2 i, CI 31 , and A are available from velocity calculations. The joint 
accelerations thus require a total of 20 multiplications and 19 additions. 
These figures represent substantial savings when compared with the num- 
bers of operations required if plain Gaussian elimination were used, namely, 
33 multiplications and 35 additions. 

It is noteworthy that in the foregoing algorithm, we have replaced neither 
the sum 0i + 02 + 0s nor <?iE(si + S 2 + S 3 ) by uj and correspondingly, by 
p, because in path tracking, there is no perfect match between joint and 
Cartesian variables. In fact, joint-rate and joint-acceleration calculations 
are needed in feedback control schemes to estimate the position, velocity, 
and acceleration errors by proper corrective actions. 
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4-8.4 Static Analysis 

Here we assume that a planar wrench acts at the end-effector of the ma- 
nipulator appearing in Fig. 4.24. In accordance with the definition of the 
planar twist in Subsection 4.8.2, eq.(4.105), the planar wrench is now de- 
fined as 

w = ” (4.113) 

where n is the scalar couple acting on the end-effector and f is the 2- 
dimensional force acting at the operation point P of the end-effector. If 
additionally, we denote by r the 3-dimensional vector of joint torques, the 
planar counterpart of eq.(4.95) follows, i.e., 

J^w = T (4.114) 



where 
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Now, in order to solve for the wrench w acting on the end-effector, given the 
joint torques r and the posture of the manipulator, we can still apply our 
compact Gaussian-elimination scheme, as introduced in Subsection 4.8.2. 
To this end, we subtract the first scalar equation from the second and the 
third scalar equations of eq.(4.114), which renders the foregoing system in 
the form 



A 
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(Esi)^ 1 
[E(s2-Si)F 
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f 
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t2 - n 
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[E(s3-Si)FJ 




1 




_t~ 3 - n_ 



Thus, the last two equations have been decoupled from the first one, which 
allows us to solve them separately, i.e., we have reduced the system to one 
of two equations in two unknowns, namely. 



r[E(s 2 -si)F] 


f — 


t 2 - n 


[[E(s3-Si)FJ 


1 — 


T3 - Ti_ 



from which we readily obtain 



r[E(s2-si)F] 


-1 


t2 - n 


[[E(s3-Si)FJ 




_T3 -Tl_ 



and hence, upon expansion of the above inverse, 

f = ^ [{t2 - ri)(s3 - Si) - (t 3 - ti)(s 2 - Si)] 



(4.115) 



(4.116) 



(4.117) 



where A is exactly as defined in eq.(4.109). Finally, the resultant moment n 
acting on the end-effector is readily calculated from the first scalar equation 
of eq.(4.114), namely, as 

n = Ti + Ef 
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thereby completing the static analysis of the manipulator under study. A 
quick analysis of computational costs shows that the foregoing solution 
needs 8M and 6A, or a savings of about 70% if straightforward Gaussian 
elimination is applied. 



4.9 Kinetostatic Performance Indices 

The balance of Part I of the book does not depend on this section, which can 
thus be skipped. We have included it here because (*) it is a simple matter to 
render the section self-contained, while introducing the concept of condition 
number and its relevance in robotics; (n) kinetostatic performance can be 
studied with the background of the material included up to this section; 
and {in) kinetostatic performance is becoming increasingly relevant as a 
design criterion and as a figure of merit in robot control. 

A kinetostatic performance index of a robotic mechanical system is a 
scalar quantity that measures how well the system behaves with regard to 
force and motion transmission, the latter being understood in the differen- 
tial sense, i.e., at the velocity level. Now, a kinetostatic performance index, 
or kinetostatic index for brevity, may be needed to assess the performance 
of a robot at the design stage, in which case we need a posture-independent 
index. In this case, the index becomes a function of the robot architecture 
only. If, on the other hand, we want to assess the performance of a given 
robot while performing a task, what we need is a posture-dependent index. 
In many instances, this difference is not mentioned in the robotics liter- 
ature, although it is extremely important. Moreover, while performance 
indices can be defined for all kinds of robotic mechanical systems, we fo- 
cus here on those associated with serial manipulators, which are the ones 
studied most intensively. 

Among the various performance indices that have been proposed, one 
can cite the concept of service angle, first introduced by Vinogradov et 
al. (1971), and the conditioning of robotic manipulators, as proposed by 
Yang and Lai (1985). Yoshikawa (1985), in turn, introduced the concept of 
manipulability, which is defined as the square root of the determinant of the 
product of the manipulator Jacobian by its transpose. Paul and Stevenson 
(1983) used the absolute value of the determinant of the Jacobian to assess 
the kinematic performance of spherical wrists. Note that for square Jaco- 
bians, Yoshikawa’s manipulability is identical to the absolute value of the 
determinant of the Jacobian, and hence, the latter coincides with Paul and 
Stevenson’s performance index. It should be pointed out that these indices 
were defined for control purposes and hence, are posture-dependent. Ger- 
mane to these concepts is that of dextrous workspace, introduced by Kumar 
and Waldron (1981), and used for geometric optimization by Vijaykumar 
et al. (1986). Although the concepts of service angle and manipulability are 
clearly different, they touch upon a common underlying issue, namely, the 
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kinematic, or alternatively, the static performance of a manipulator from 
an accuracy viewpoint. 

What is at stake when discussing the manipulability of a robotic manip- 
ulator is a measure of the invertibility of the associated Jacobian matrix, 
since this is required for velocity and force-feedback control. One further 
performance index is based on the condition number of the Jacobian, which 
was first used by Salisbury and Craig (1982) to design mechanical fingers. 
Here, we shall call such an index the conditioning of the manipulator. For 
the sake of brevity, we devote the discussion below to only two indices, 
namely, manipulability and conditioning. Prior to discussing these indices, 
we recall a few facts from linear algebra. 

Although the concepts discussed here are equally applicable to square 
and rectangular matrices, we shall focus on the former. First, we give a 
geometric interpretation of the mapping induced by an n x n matrix A. 
Here, we do not assume any particular structure of A, which can thus 
be totally arbitrary. However, by invoking the polar- decomposition theorem 
(Strang, 1988), we can factor A as 

A = RU = VR (4.118) 

where R is orthogonal, although not necessarily proper, while U and V are 
both at least positive-semidefinite. Moreover, if A is nonsingular, then U 
and V are both positive-definite, and R is unique. Clearly, U can be readily 
determined as the positive-semidefinite or correspondingly, positive-definite 
square root of the product A^A, which is necessarily positive-semidefinite; 
it is, in fact, positive-definite if A is nonsingular. We recall here that the 
square root of arbitrary matrices was briefly discussed in Subsection 2.3.6. 
The square root of a positive-semidefinite matrix can be most easily under- 
stood if that matrix is assumed to be in diagonal form, which is possible 
because such a matrix is necessarily symmetric, and every symmetric ma- 
trix is diagonalizable. The matrix at hand being positive-semidefinite, its 
eigenvalues are nonnegative, and hence, their square roots are all real. The 
positive-semidefinite square root of interest is, then, readily obtained as the 
diagonal matrix whose nontrivial entries are the nonnegative square roots 
of the aforementioned eigenvalues. With U determined, R can be found 
uniquely only if A is nonsingular, in which case U is positive-definite. If 
this is the case, then we have 

R = U-^A (4.119a) 

It is a simple matter to show that V can be found, in turn, as a similarity 
transformation of U, namely, as 

V = RUR^ (4.119b) 

Now, let vector x be mapped by A into z, i.e.. 



z = Ax = RUx 



(4.120a) 
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Moreover, let 

y = Ux (4.120b) 

and hence, we have a concatenation of mappings, namely, U maps x into 
y, while R maps y into z. Thus, by virtue of the nature of matrices R 
and U, the latter maps the unit n-dimensional ball into an n-axis ellipsoid 
whose semiaxis lengths bear the ratios of the eigenvalues of U. Moreover, 
R maps this ellipsoid into another one with identical semiaxes, except that 
it is rotated about its center or reflected, depending upon whether R is 
proper or improper orthogonal. In fact, the eigenvalues of U or, for that 
matter, those of V, are nothing but the singular values of A. Yoshikawa 
(1985) explained the foregoing relations resorting to the singular-value de- 
composition theorem. We prefer to invoke the polar-decomposition theorem 
instead, because of the geometric nature of the latter, as opposed to the 
former, which is of an algebraic nature — it is based on a diagonalization of 
either U or V, which is really not needed. 

We illustrate the two mappings U and R in Fig. 4.27, where we orient 
the X, Y, and Z axes along the three eigenvectors of U. Therefore, the 
semi axes of the ellipsoid are oriented as the eigenvectors of U as well. If 
A is singular, then the ellipsoid degenerates into one with at least one 
vanishing semiaxis. On the other hand, if matrix A is isotropic, i.e., if all 
its singular values are identical, then it maps the unit ball into another 
ball, either enlarged or shrunken. 

For our purposes, we can regard the Jacobian of a serial manipulator as 
mapping the unit ball in the space of joint rates into a rotated or reflected 
ellipsoid in the space of Cartesian velocities, or twists. Now, let us assume 
that the polar decomposition of J is given by R and U, the manipulability 
p of the robot under study thus becoming 

p = |det(J)| = |det(R)||det(U)| (4.121a) 

Since R is orthogonal, the absolute value of its determinant is unity. 




FIGURE 4.27. Geometric representation of mapping induced by matrix A. 
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Additionally, the determinant of U is nonnegative, and hence, 

jj = det(U) (4.121b) 

which shows that the manipulability is the product of the eigenvalues of 
U or equivalently, of the singular values of J. Now, the product of those 
singular values, in the geometric interpretation of the mapping induced by 
J, is proportional to the volume of the ellipsoid at hand, and hence, p, can 
be interpreted as a measure of the volume of that ellipsoid. It is apparent 
that the manipulability defined in eq.(4.121b) is posture-dependent. For 
example, if J is singular, at least one of the semiaxes of the ellipsoid van- 
ishes, and so does its volume. Manipulators at singular configurations thus 
have a manipulability of zero. 

Now, if we want to use the concept of manipulability to define a posture- 
independent kinetostatic index, we have to define this index in a global 
sense. This can be done in the same way as the magnitude of a vector is 
defined, namely, as the sum of the squares of its components. In this way, 
the global manipulability can be defined as the integral of a certain power 
of the manipulability over the whole workspace of the manipulator, which 
would amount to defining the index as a norm of the manipulability in a 
space of functions. For example, we can use the maximum manipulability 
attained over the whole workspace, thereby ending up with what would 
be like a Chebyshev norm; alternatively, we can use the root-mean square 
(rms) value of the manipulability, thereby ending up with a measure similar 
to the Euclidean norm. 

Furthermore, if we have a Jacobian J whose entries all have the same 
units, then we can define its condition number k(J) as the ratio of the 
largest singular value <t; of J to the smallest one, <Ts, i.e., 

k(J) = — (4.122) 

0's 

Note that k(J) can attain values from 1 to infinity. Clearly, the condition 
number attains its minimum value of unity for matrices with identical sin- 
gular values; such matrices map the unit ball into another ball, although 
of a different size, and are, thus, called isotropic. By extension, isotropic 
manipulators are those whose Jacobian matrix can attain isotropic values. 
On the other side of the spectrum, singular matrices have a smallest singu- 
lar value that vanishes, and hence, their condition number is infinity. The 
condition number of J can be thought of as indicating the distortion of 
the unit ball in the space of joint- variables. The larger this distortion, the 
greater the condition number, the worst-conditioned Jacobians being those 
that are singular. For these, one of the semiaxes of the ellipsoid vanishes 
and the ellipsoid degenerates into what would amount to an elliptical disk 
in the 3-dimensional space. 

The condition number of a square matrix can also be understood as a 
measure of the relative roundoff-error amplification of the computed results 
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upon solving a linear system of equations associated with that matrix, with 
respect to the relative roundoff error of the data (Dahlquist and Bjorck, 
1974; Golub and van Loan, 1989). Based on the condition number of the 
Jacobian, a posture-independent kinematic conditioning index of robotic 
manipulators can now be defined as a global measure of the condition 
number, or its reciprocal for that matter, which is better behaved because 
it is bounded between 0 and unity. 

Now, if the entries of J have different units, the foregoing definition of 
k(J) cannot be applied, for we would face a problem of ordering singular 
values of different units from largest to smallest. We resolve this inconsis- 
tency by defining a characteristic length, by which we divide the Jacobian 
entries that have units of length, thereby producing a new Jacobian that 
is dimensionally homogeneous. We shall therefore divide our study into (*) 
manipulators for only positioning tasks, (m) manipulators for only orien- 
tation tasks, and (in) manipulators for both positioning and orientation 
tasks. The characteristic length will be introduced when stud}4ng the third 
category. 

In the sequel, we will need an interesting property of isotropic matri- 
ces that is recalled below. First note that given the polar decomposition 
of a square matrix A of eq.(4.118), its singular values are simply the — 
nonnegative — eigenvalues of matrix U, or those of V, for both matrices 
have identical eigenvalues. Moreover, if A is isotropic, all the foregoing 
eigenvalues are identical, say equal to <t, and hence, matrices U and V are 
proportional to the n x n identity matrix, i.e., 

U = V = crl (4.123) 

In this case, then, 

A = (tR 

which means that isotropic square matrices 
matrices. As a consequence, then, 

A^A = a^l (4.124b) 

Given an arbitrary manipulator of the serial type with a Jacobian matrix 
whose entries all have the same units, we can calculate its condition number 
and use a global measure of this to define a posture-independent kineto- 
static index. Let Km be the minimum value attained by the condition num- 
ber of the dimensionally homogeneous Jacobian over the whole workspace. 
Note that 1/Km can be regarded as a Chebyshev norm of the reciprocal of 
k(J), because now 1/Km represents the maximum value of this reciprocal 
in the whole workspace. We then introduce a posture-independent perfor- 
mance index, the kinematic conditioning index, or KCI for brevity, defined 

KCI = — X 100 



(4.124a) 

are proportional to rectangular 



as 



(4.125) 
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Notice that since the condition number is bounded from below, the KCI 
is bounded from above by a value of 100%. Manipulators with a KCI of 
100% are those identified above as isotropic because their Jacobians have, 
at the configuration of minimum condition number, all their singular values 
identical and different from zero. 

While the condition number of J defined in eq.(4.122) is conceptually 
simple, for it derives from the polar-decomposition theorem, it is by no 
means computationally simple. First, it relies on the eigenvalues of J^J, 
which only in special cases can be found in symbolic form; second, even 
if eigenvalues are available symbolically, their ordering from smallest to 
largest varies with the manipulator architecture and posture. An alternative 
definition of k(J) that is computationally simpler relies on the general 
definition of the concept, namely (Golub and van Loan, 1989), 

k(J) = ||J||||J-i|| (4.126a) 

where || • || stands for the matrix norm (Golub and van Loan, 1989). While 
any norm can be used in the above definition, the one that is most conve- 
nient for our purposes is the Frobenius norm || • ||i?, defined as 



IIJIIf 



-tr(JJ^) 

n 



(4.126b) 



where we have assumed that J is of n x n. Although a symbolic expres- 
sion for is not always possible, this expression is more frequently 
available than one for the eigenvalues of J^J. Moreover, from the polar- 
decomposition theorem and Theorem 2.6.3, one can readily verify that 
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n 



(4.126c) 



4-9.1 Positioning Manipulators 

Here, again, we shall distinguish between planar and spatial manipulators. 
These are studied separately. 

Planar Manipulators 

If the manipulator of Fig. 4.24 is limited to positioning tasks, we can dis- 
pense with its third axis, the manipulator thus reducing to the one shown 
in Fig. 4.25; its Jacobian reduces correspondingly to 

J = [Esi Es 2 ] 

with Si denoting the two-dimensional versions of vectors r* of the Denavit- 
Hartenberg notation, as introduced in Fig. 4.19. Now, if we want to design 
this manipulator for maximum manipulability, we need first to determine its 
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manipulability as given by eq.(4.121a) or correspondingly, as p, = |det(J)|. 
Now, note that 

det(J) = det(E [si S 2 ]) = det(E)det([si S 2 ]) 

and since matrix E is orthogonal, its determinant equals unity. Thus, the 
determinant of interest is now calculated using Fact 4.8.3 of Section 4.8, 
namely, 

det(J) = — sfEs 2 (4.127) 

Therefore, 

p = |sfEs 2 | = ||si||||s 2 |||sin(si, 82 )! 

where (si, 82 ) stands for the angle between the two vectors inside the paren- 
theses. Now let us denote the manipulator reach with R, i.e., i? = ai + 02 , 
and let aj, = Rpk, where pk, for A: = 1,2, is a dimensionless number. As 
the reader can readily verify, p turns out to be twice the area of triangle 
O 1 O 2 P, with the notation adopted at the outset. Hence, 

p = i?^piP 2 | sin 6 » 2 | (4.128) 

with Pi and p 2 subjected to 

Pi+P2 = l (4.129) 

The design problem at hand, then, can be formulated as an optimization 
problem aimed at maximizing p as given in eq.(4.128) over p\ and p 2 , 
subject to the constraint (4.129). This optimization problem can be readily 
solved using, for example, Lagrange multipliers, thereby obtaining 

Pi= P2 = O 2 = ±- 

the absolute value of sin O 2 attaining its maximum value when O 2 = ±90° . 
The maximum manipulability thus becomes 

Mmax = — (4.130) 

Incidentally, the equal-length condition maximizes the workspace volume 
as well. 

On the other hand, if we want to minimize the condition number of J, 
we should aim at rendering it isotropic, which means that the product J 
should be proportional to the identity matrix, and so. 
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where a is the repeated singular value of J. Hence, for J to be isotropic, 
all we need is that the two vectors si and 82 have the same norm and that 
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FIGURE 4.28. A two-axis isotropic manipulator. 

they lie at right angles. The solution is a manipulator with link lengths 
observing a ratio of %/2/2, i.e., with aija^ = %/2/2, and the two link axes 
at an angle of 135°, as depicted in Fig. 4.28. Manipulators of the above 
type, used as mechanical fingers, were investigated by Salisburg and Craig 
(1982), who found that these manipulators can be rendered isotropic if 
given the foregoing dimensions and configured as shown in Fig. 4.28. 

Spatial Manipulators 

Now we have a manipulator like that depicted in Fig. 4.9, its Jacobian 
matrix taking on the form 

J = [eixri 62 X T 2 eaxra] (4.131) 

The condition for isotropy of this kind of manipulator takes on the form of 
eq.(4.124b), which thus leads to 

3 

X Tk){ek X Tk)'^ = cr^l (4.132) 

1 

This condition can be attained by various designs, one example being the 
manipulator of Fig. 4.15. Another isotropic manipulator for 3-dimensional 
positioning tasks is displayed in Fig. 4.29. 

Note that the manipulator of Fig. 4.29 has an orthogonal architecture, 
the ratio of its last link length to the length of the intermediate link being, 
as in the 2-dimensional case, %/2/2. Since the first axis does not affect 
singularities, neither does it affect isotropy, and hence, not only does one 
location of the operation point exist that renders the manipulator isotropic, 
but a whole locus, namely, the circle known as the isotropy circle, indicated 
in the same figure. By the same token, the manipulator of Fig. 4.28 has 
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isotr oyy circle 



X 




FIGURE 4.29. An isotropic manipulator for 3-dimensional positioning tasks, 
an isotropy circle centered at the center of the first ioint, with a radius of 

(V2/2)ai. 



4-9.2 Orienting Manipulators 

We now have a three-revolute manipulator like that depicted in Fig. 4.17, 
its Jacobian taking on the simple form 

J=[ei 62 63 ] (4.133) 

and hence, the isotropy condition of eq. (4.124b) leads to 

3 

^ 6 fce^ = Xi (4.134) 

1 

What the foregoing condition states is that a spherical wrist for orienting 
tasks is isotropic if its three unit vectors {es,}® are so laid out that the three 
products 6 fce^, for k = 1,2, 3, add up to a multiple of the 3x3 identity 
matrix. This is the case if the three foregoing unit vectors are orthonor- 
mal, which occurs in orthogonal wrists when the two planes defined by the 
corresponding pairs of neighboring axes are at right angles. Moreover, the 
value of a in this case can be readily found if we take the trace of both 




180 



4. Kinetostatics of Simple Robotic Manipulators 



sides of the above equation, which yields 

3 

• Gk = (4.135) 

1 

and hence, <t = 1, because all three vectors on the left-hand side are of unit 
magnitude. In summary, then, orthogonal wrists, which are rather frequent 
among industrial manipulators, are isotropic. Here we have an example of 
engineering insight leading to an optimal design, for such wrists existed 
long before isotropy was introduced as a design criterion for manipula- 
tors. Moreover, notice that from the results of Subsection 4.4.2, spherical 
manipulators with an orthogonal architecture have a maximum workspace 
volume. That is, isotropic manipulators of the spherical type have two 
optimality properties: they have both a maximum workspace volume and 
a maximum KCI. Apparently, the manipul ability of orthogonal spherical 
wrists is also optimal, as the reader is invited to verify, when the wrist is 
postured so that its three axes are mutually orthogonal. In this posture, 
the manipulability of the wrist is unity. 

4-9.3 Positioning and Orienting Manipulators 

We saw already in Subsubsection 4.9.1 that the optimization of the two in- 
dices studied here — the condition number of the Jacobian matrix and the 
manipulability — leads to different manipulators. In fact, the two indices 
entail even deeper differences, as we shall see presently. First and foremost, 
as we shall prove for both planar and spatial manipulators, the manipula- 
bility p is independent of the operation point P of the end-effector, while 
the condition number is not. One more fundamental difference is that while 
calculating the manipulability of manipulators meant for both positioning 
and orienting tasks poses no problem, the condition number cannot be 
calculated, at least directly, for this kind of manipulator. Indeed, in order 
to determine the condition number of the Jacobian matrix, we must or- 
der its singular values from largest to smallest. However, in the presence 
of positioning and orienting tasks, three of these singular values, namely, 
those associated with orientation, are dimensionless, while those associated 
with positioning have units of length, thereby making impossible such an 
ordering. We resolve this dimensional inhomogeneity by introducing a nor- 
malizing characteristic length. Upon dividing the three positioning rows, 
i.e., the bottom rows, of the Jacobian by this length, a nondimensional Ja- 
cobian is obtained whose singular values are nondimensional as well. The 
characteristic length is then defined as the normalizing length that renders 
the condition number of the Jacobian matrix a minimum. Below we shall 
determine the characteristic length for isotropic manipulators; determin- 
ing the same for nonisotropic manipulators requires solving a minimization 
problem that calls for numerical techniques, as illustrated with an example. 
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Planar Manipulators 



In the ensuing development, we will need the planar counterpart of the 
twist-transfer formula of Subsection 3.4.2. First, we denote the 3-dimen- 
sional twist of a rigid body undergoing planar motion, defined at a point 
A, by t^; when defined at point B, the corresponding twist is denoted by 
t_B, i.e.. 



tA = 



LU 

a 




(4.136) 



The relation between the two twists, or the planar twist-transfer formula, 
is given by a linear transformation U as 



t_B = Ut^ 



(4.137a) 



where U is now defined as 

U 



1 0 ^' 
E(b - a) I2 _ 



(4.137b) 



with a and b representing the position vectors of points A and B, and I2 
stands for the 2x2 identity matrix. Moreover, U is, not surprisingly, a 
member of the 3x3 unimodular group, i.e.. 



det(U) = 1 



Because of the planar twist-transfer formula, the Jacobian defined at an 
operation point B is related to that defined at an operation point A of the 
same end-effector by the same linear transformation U, i.e., if we denote 
the two Jacobi ans by Ja and Jb, then 



Jb = UJa (4.138) 

and if we denote by pA and pB the manipulability calculated at points A 
and B, respectively, then 



Pb = |det(J_B)| = |det(U)||det(JA)| = |det(JA)| = PA (4.139) 



thereby proving that the manipulability is insensitive to a change of op- 
eration point, or to a change of end-effector, for that matter. Note that a 
similar analysis for the condition number cannot be completed at this stage 
because as pointed out earlier, the condition number of these Jacobian ma- 
trices cannot even be calculated directly. 

In order to resolve the foregoing dimensional inhomogeneity, we introduce 
the characteristic length L, which will be defined as that rendering the 
Jacobian dimensionally homogeneous and optimally conditioned, i.e., with 
a minimum condition number. We thus redefine the Jacobian matrix of 
interest as 



J = 



L L 



Eri 



1 

lEr2 



1 

lErg 



(4.140) 
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Now, if we want to size the manipulator at hand by properly choosing 
its geometric parameters so as to render it isotropic, we must observe the 
isotropy condition, eq.(4.124b), which readily leads to 
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,lEE?rfc ^EE?(rfcrD]E^_ 



ct2 0 0 

0 ct2 0 
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and hence. 
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(4.141) 



(4.142a) 

(4.142b) 

(4.142c) 



What eq.(4.142a) states is simply that the triple singular value of the 
isotropic J is a/3; eq.(4.142b) states, in turn, that the operation point is 
the centroid of the centers of all manipulator joints if its Jacobian matrix 
is isotropic. Now, in order to gain more insight into eq.(4.142c), we note 
that since E is orthogonal and = 3, this equation can be rewritten in a 
simpler form, namely. 



= (3)12 (4.143) 

Further, if we recall the definition of the moment of inertia of a rigid body, 
we can immediately realize that the moment of inertia Ip of a set of par- 
ticles of unit mass located at the centers of the manipulator joints, with 
respect to the operation point P, is given by 

1^^ = XI -rfcr^) (4.144) 

1 

from which it is apparent that the moment of inertia of the set comprises 
two parts, the first being isotropic — it is a multiple of the 2x2 identity 
matrix — the second not necessarily so. However, the second part has the 
form of the left-hand side of eq.(4.143). Hence, eq.(4.143) states that if the 
manipulator under study is isotropic, then its joint centers are located, at 
the isotropic configuration, at the corners of a triangle that has circular 
inertial symmetry. What we mean by this is that the 2x2 moment of 
inertia of the set of particles, with entries I^x, Ixy, and lyy, is similar to 
that of a circle, i.e., with I^x = lyy and I^y = 0. An obvious candidate 
for such a triangle is, obviously, an equilateral triangle, the operation point 
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thus coinciding with the center of the triangle. Since the corners of an 
equilateral triangle are at equal distances d from the center, and these 
distances are nothing but ||rfc||, then the condition below is readily derived 
for isotropy: 

||rfc|p = d^ (4.145) 

In order to compute the characteristic length of the manipulator under 
study, let us take the trace of both sides of eq.(4.143), thereby obtaining 



and hence, upon substituting eq.(4.145) into the foregoing relation, an ex- 
pression for the characteristic length, as pertaining to planar isotropic ma- 
nipulators, is readily derived, namely. 




(4.146) 



It is now a simple matter to show that the three link lengths of this isotropic 
manipulator are a\ = = \f^d and = d. Such a manipulator is sketched 

in an isotropic configuration in Fig. 4.30. 



Spatial Manipulators 

The entries of the Jacobian of a six-axis manipulator meant for both po- 
sitioning and orienting tasks are dimensionally inhomogeneous as well. In- 
deed, as discussed in Section 4.5, the *th column of J is composed of the 




FIGURE 4.30. The planar 3-R isotropic manipulator. 
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Plucker coordinates of the *th axis of the manipulator, namely, 

j ©2 ©3 ©4 ©5 ^6 147) 

©1 X ri ©2 X T 2 ©3 X T 3 ©4 X T 4 ©5 X T 5 ©6 X Te 

Now it is apparent that the first three rows of J are dimensionless, 
whereas the remaining three, corresponding to the moments of the axes 
with respect to the operation point of the end-effector, have units of length. 
This dimensional inhomogeneity is resolved in the same way as in the case 
of planar manipulators for both positioning and orienting tasks, i.e., by 
means of a characteristic length. This length is defined as the one that 
minimizes the condition number of the dimensionless Jacobian thus ob- 
tained. We then redefine the Jacobian as 

J _ ei ©2 ©3 ©4 ©5 66 

“|_i©ixri i ©2 X T 2 i©3 X T3 i©4 X T4 ^©5 X Ts ^©6 X Te 

(4.148) 

and hence, the isotropy condition of eq.(4.124b) leads to 

^©fc©^ = cr^i 
1 

6 

^©fc(©fc X Ykf' = O 
1 

1 ® 

^( 6 fc X rfc)(©fc X Ykf = cr^l 

where 1 is the 3x3 identity matrix, and O is the 3x3 zero matrix. Now, 
if we take the trace of both sides of eq.(4.149a), we obtain 

(T^ = 2 or (T = %/2 

Furthermore, we take the trace of both sides of eq.(4.149c), which yields 

^^||©fc X Ykf = 3cr2 

But ||©fc X rfclp is nothing but the square of the distance dk of the A:th 
revolute axis to the operation point, the foregoing equation thus yielding 

L = 

i.e., the characteristic length of a spatial six-revolute isotropic manipulator 
is the root-mean square of the distances of the revolute axes to the opera- 
tion point when the robot finds itself at the posture of minimum condition 
number. 




(4.149a) 

(4.149b) 

(4.149c) 
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Furthermore, eq.(4.149a) states that if {es, }® is regarded as the set of 
position vectors of points { Pk }® on the surface of the unit sphere, then 
the moment-of-inertia matrix of the set of equal masses located at these 
points has spherical symmetry. What the latter means is that any direction 
of the 3-dimensional space is a principal axis of inertia of the foregoing 
set. Likewise, eq.(4.149c) states that if {es, x rj, }® is regarded as the set of 
position vectors of points { Qa: } in the 3-dimensional Euclidean space, then 
the moment-of-inertia matrix of the set of equal masses located at these 
points has spherical symmetry. 

Now, in order to gain insight into eq.(4.149b), let us take the axial vector 
of both sides of that equation, thus obtaining 

6 

X (efc X rfc) = 0 (4.150) 

1 

with 0 denoting the 3-dimensional zero vector. Furthermore, let us denote 
by Efc the cross-product matrix of e^, the foregoing equation thus taking 
on the form 

1 

Flowever, 

Efc = -1 + ekoj. 

for every k, and hence, eq.(4.150) leads to 

6 

E(1 -efce^)rfc = 0 
1 

Moreover, (1 — ekej)rk is nothing but the normal component of with 
respect to e^, as defined in Section 2.2. Let us denote this component by 
r^, thereby obtaining an alternative expression for the foregoing equation, 
namely, 

6 

Erfc=0 (4.151) 

1 

The geometric interpretation of the foregoing equation is readily derived: 
To this end, let 0(, be the foot of the perpendicular to the A:th revolute 
axis from the operation point P; then, is the vector directed from 0(, to 
P. Therefore, the operation point of an isotropic manipulator, configured 
at the isotropic posture is the centroid of the set { 0(, }® of perpendicular- 
feet from the operation point. 

A six-axis manipulator designed with an isotropic architecture, DIE- 
STRO, is displayed in Fig. 4.31. The Denavit-Flartenberg parameters of this 
manipulator are given in Table 4.1. DIESTRO is characterized by identical 
link lengths a and offsets identical with this common link length, besides 
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TABLE 4.1. DH Parameters of DIESTRO 



i 


ai (mm) 


bi (mm) 


CXi 


Oi 


1 


50 


50 


90° 


0i 


2 


50 


50 


-90° 


O2 


3 


50 


50 


90° 


O3 


4 


50 


50 


-90° 


O4 


5 


50 


50 


90° 


Ob 


6 


50 


50 


-90° 


Oe 




EIGURE 4.31. DIESTRO, a six-axis isotropic manipulator. 

twist angles of 90° between all pairs of neighboring axes. Not surprisingly, 
the characteristic length of this manipulator is a. 

Example 4.9.1 Find the KCI and the characteristic length of the Fanuc 
Arc Mate robot whose DFl parameters are given in Table 4.2. 

Solution: Apparently, what we need is the minimum value rcmin that the 
condition number of the manipulator Jacobian can attain, in order to cal- 
culate its KCI as indicated in eq.(4.125). Now, the Fanuc Arc Mate robot 
is a six-revolute manipulator for positioning and orienting tasks. Hence, 
its Jacobian matrix has to be first recast in nondimensional form, as in 
eq.(4.148). Next, we find L, along with the joint variables that determine 
the posture of minimum condition number via an optimization procedure. 
Prior to the formulation of the underlying optimization problem, however, 
we must realize that the first joint, accounting for motions of the manipu- 
lator as a single rigid body, does not affect its Jacobian condition number. 
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We thus define the design vector x of the optimization problem at hand as 
X = [ 02 03 Si 05 06 L] 

and set up the optimization problem as 

min k(J) 

X 

The condition number having been defined as the ratio of the largest to the 
smallest singular values of the Jacobian matrix at hand, the gradient of the 
above objective function, drc/dx, is apparently elusive to calculate. Thus, 
we use a direct-search method, i.e., a method not requiring any partial 
derivatives, but rather, only objective-function evaluations, to solve the 
above optimization problem. There are various methods of this kind at 
our disposal; the one we chose is the simplex method, as implemented in 
Matlab. The results reported are displayed below: 

Xopt = [26.82° -56.06° 15.79° -73.59° -17.83° 0.3573] 

where the last entry, the characteristic length of the robot, is in meters, 
i.e., 

L = 357.3 mm 

Furthermore, the minimum condition number attained at the foregoing 
posture, with the characteristic length found above, is 

Urn = 2.589 

Therefore, the KCI of the Fanuc Arc Mate is 

KCI = 38.625% 

and so this robot is apparently far from being kinematically isotropic. To 
be sure, the KCI of this manipulator can still be improved dramatically by 
noting that the condition number is highly dependent on the location of the 
operation point of the end-effector. As reported by Tandirci et al. (1992), 
an optimum selection of the operation point for the robot at hand yields a 

TABLE 4.2. DH Parameters of the Fanuc Arc Mate Manipulator 



i 


Oi (mm) 


bi (mm) 


CXi 


0i 


1 


200 


810 


90° 


01 


2 


600 


0 


0° 


02 


3 


130 


30 


90° 


03 


4 


0 


550 


90° 


04 


5 


0 


100 


90° 


0B 


6 


0 


100 


0° 


06 
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minimum condition number of 1.591, which thus leads to a KCI of 62.85%. 
The point of the EE that yields the foregoing minimum is thus termed 
the characteristic point of the manipulator in the foregoing reference. Its 
location in the EE is given by the DH parameters and b^, namely, 

a,e = 223.6 mm, = 274.2 mm 




5 



Trajectory Planning: Pick-and-Place 
Operations 



5.1 Introduction 

The motions undergone by robotic mechanical systems should be, as a rule, 
as smooth as possible; i.e., abrupt changes in position, velocity, and acceler- 
ation should be avoided. Indeed, abrupt motions require unlimited amounts 
of power to be implemented, which the motors cannot supply because of 
their physical limitations. On the other hand, abrupt motion changes arise 
when the robot collides with an object, a situation that should also be 
avoided. While smooth motions can be planned with simple techniques, as 
described below, these are no guarantees that no abrupt motion changes will 
occur. In fact, if the work environment is cluttered with objects, whether 
stationary or mobile, collisions may occur. Under ideal conditions, a flexible 
manufacturing cell is a work environment in which all objects, machines 
and workpieces alike, move with preprogrammed motions that by their 
nature, can be predicted at any instant. Actual situations, however, are 
far from being ideal, and system failures are unavoidable. Unpredictable 
situations should thus be accounted for when designing a robotic system, 
which can be done by suppl}dng the system with sensors for the automatic 
detection of unexpected events or by providing for human monitoring. Nev- 
ertheless, robotic systems find applications not only in the well-structured 
environments of flexible manufacturing cells, but also in unstructured en- 
vironments such as exploration of unknown terrains and systems in which 
humans are present. The planning of robot motions in the latter case is 
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obviously much more challenging than in the former. Robot motion plan- 
ning in unstructured environments calls for techniques beyond the scope 
of those studied in this book, involving such areas as pattern recognition 
and artificial intelligence. For this reason, we have devoted this book to the 
planning of robot motions in structured environments only. 

Two typical tasks call for trajectory planning techniques, namely, 

• pick-and-place operations (PPO), and 

• continuous paths (CP). 

We will study PPO in this chapter, with Chapter 9 devoted to CP. More- 
over, we will focus on simple robotic manipulators of the serial type, al- 
though these techniques can be directly applied to other, more advanced, 
robotic mechanical systems. 



5.2 Background on PPO 

In PPO, a robotic manipulator is meant to take a workpiece from a given 
initial pose, specified by the position of one of its points and its orienta- 
tion with respect to a certain coordinate frame, to a final pose, specified 
likewise. However, how the object moves from its initial to its final pose is 
immaterial, as long as the motion is smooth and no collisions occur. Pick- 
and-place operations are executed in elementary manufacturing operations 
such as loading and unloading of belt conveyors, tool changes in machine 
tools, and simple assembly operations such as putting roller bearings on a 
shaft. The common denominator of these tasks is material handling, which 
usually requires the presence of conventional machines whose motion is very 
simple and is usually characterized by a uniform velocity. In some instances, 
such as in packing operations, a set of workpieces, e.g., in a magazine, is 
to be relocated in a prescribed pattern in a container, which constitutes 
an operation known as palletizing. Although palletizing is a more elaborate 
operation than simple pick-and-place, it can be readily decomposed into a 
sequence of the latter operations. 

It should be noted that although the initial and the final poses in a PPO 
are prescribed in the Cartesian space, robot motions are implemented in 
the joint space. Hence, the planning of PPO will be conducted in the latter 
space, which brings about the need of mapping the motion thus planned 
into the Cartesian space, in order to ensure that the robot will not collide 
with other objects in its surroundings. The latter task is far from being 
that simple, since it involves the rendering of the motion of all the moving 
links of the robot, each of which has a particular geometry. An approach to 
path planning first proposed by Lozano-Perez (1981) consists of mapping 
the obstacles in the joint space, thus producing obstacles in the joint space 
in the form of regions that the joint-space trajectory should avoid. The 
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FIGURE 5.1. Still image of the animation of a palletizing operation. 

idea can be readily implemented for simple planar motions and simple ge- 
ometries of the obstacles. However, for general 3-D motions and arbitrary 
geometries, the computational requirements make the procedure imprac- 
tical. A more pragmatic approach would consist of two steps, namely, (*) 
planning a preliminary trajectory in the joint space, disregarding the obsta- 
cles, and (m) visually verifying if collisions occur with the aid of a graphics 
system rendering the animation of the robot motion in the presence of 
obstacles. The availability of powerful graphics hardware enables the fast 
animation of robot motions within a highly realistic environment. Shown 
in Fig. 5.1 is a still image of the animation produced by RVS, the McGill 
University Robot- Visualization System, of the motion of a robot performing 
a palletizing operation. Commercial software for robot-motion rendering is 
available. 

By inspection of the kinematic closure equations of robotic manipulators — 
see eqs.(4.5a & b) — it is apparent that in the absence of singularities, 
the mapping of joint to Cartesian variables, and vice versa, is continu- 
ous. Hence, a smooth trajectory planned in the joint space is guaranteed 
to be smooth in the Cartesian space, and the other way around, as long as 
the trajectory does not encounter a singularity. 

In order to proceed to synthesize the joint trajectory, we must then start 
by mapping the initial and final poses of the workpiece, which is assumed 
to be rigidly attached to the EE of the manipulator, into manipulator 
configurations described in the joint space. This is readily done with the 
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methods described in Chapter 4. Let the vector of joint variables at the 
initial and final robot configurations be denoted by 9j and 9p, respectively. 
Moreover, the initial pose in the Cartesian space is defined by the position 
vector p/ of the operation point P of the EE and a rotation matrix Q/. 
Likewise, the final pose in the Cartesian space is defined by the position 
vector pir of P and the rotation matrix Qjr. Moreover, let p/ and p/ 
denote the velocity and acceleration of P, while ujj and th/ denote the 
angular velocity and angular acceleration of the workpiece, all of these at 
the initial pose. These variables at the final pose are denoted likewise, with 
the subscript I changed to F. Furthermore, we assume that time is counted 
from the initial pose, i.e., at this pose, t = 0. If the operation takes place 
in time T, then at the final pose, t = T. We have thus the set of conditions 
that define a smooth motion between the initial and the final poses, namely. 



p(0) = Pi 


p(0) = 0 


p(0) = 0 (5.1a) 


Q(0) = Qi 


w(0) = 0 


th(0) = 0 (5.1b) 


p(T) = pf 


p(T) = 0 


p(T) = 0 (5.1c) 


Q(T) = Qf 


pT) = 0 


th(T) = 0 (5. Id) 



In the absence of singularities, then, the conditions of zero velocity and 
acceleration imply zero joint velocity and acceleration, and hence, 

6>(0) = 9i 9{0) = 0 0(0) = 0 (5.2a) 

9{T) = 9p 9{T) = 0 9{T) = 0 (5.2b) 

5.3 Polynomial Interpolation 

A simple inspection of conditions (5.2a) and (5.2b) reveals that a linear 
interpolation between initial and final configurations will not work here, and 
neither will a quadratic interpolation, for its slope vanishes only at a single 
point. Hence, a higher-order interpolation is needed. On the other hand, 
these conditions imply, in turn, six conditions for every joint trajectory, 
which means that if a polynomial is to be employed to represent the motion 
of every joint, then this polynomial should be at least of the fifth degree. 
We thus start by studying trajectory planning with the aid of a 5th-degree 
polynomial. 



5.3.1 A 3-4-5 Interpolating Polynomial 

In order to represent each joint motion, we use here a fifth-order polynomial 
s(t), namely. 



s(t) = aP + bP + cP + dP + er + / 



(5.3) 
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such that 



0<s<l, 0<T<1 (5.4) 

and 

t , . 

(5-5) 

We will thus aim at a normal polynomial that, upon scaling both its argu- 
ment and the polynomial itself, will allow us to represent each of the joint 
variables 9j throughout its range of motion, so that 

6»j(t) = -h {ef - e^j)s(T) (5.6a) 

where and 9^ are the given initial and final values of the jth joint 
variable. In vector form, eq.(5.6a) becomes 



e{t) = ei + {6F -ei)s{T) ( 5 . 6 b) 

and hence, 

0{t) = {Of - 9i)s'{T)f{t) = {Of - 9i)^s'{t) (5.6c) 

Likewise, 

e{t) = ^{eF-ei)s''{r) (5.6d) 

and 

m = ^i0F-0i)s'"ir) (5.6e) 

What we now need are the values of the coefficients of s(t) that appear in 
eq.(5.3). These are readily found by recalling conditions (5.2a & b), upon 
consideration of eqs.(5.6b-d). We thus obtain the end conditions for s(t), 
namely, 

s(0)=0, s'(0)=0, s"(0)=0, s(l) = 1, s'(l) = 0, s"(l)=0 

(5.7) 

The derivatives of s(t) appearing above are readily derived from eq.(5.3), 
i.e., 

s'{t) = 5ar^ + 46t® + 3ct^ + 2dr + e (5-8) 

and 

s"{t) = 20a , + 126t^ + 6ct + 2d (5-9) 

Thus, the first three conditions of eq.(5.7) lead to 



f = e = d = 0 



(5.10) 




194 



5. Trajectory Planning: Pick-and-Place Operations 



while the last three conditions yield three linear equations in a, b, and c, 
namely, 

a + 6 + c = 1 (5.11a) 

5a + 46 + 3c = 0 (5.11b) 

20a + 126 + 6c = 0 (5.11c) 

Upon solving the three foregoing equations for the three aforementioned 
unknowns, we obtain 

a = 6, 6 = —15, c = 10 (5-12) 

and hence, the normal polynomial sought is 

s(t) = 6t® — Ihr"^ + IOt® (5.13) 

which is called a 3-4-5 polynomial. 

This pol}momial and its first three derivatives, all normalized to fall 
within the ( — 1, 1) range, are shown in Fig. 5.2. Note that the smoothness 
conditions imposed at the outset are respected and that the curve thus 
obtained is a monotonically growing function of t, a rather convenient 
property for the problem at hand. 

It is thus possible to determine the evolution of each joint variable if we 
know both its end values and the time T required to complete the motion. 
If no extra conditions are imposed, we then have the freedom to perform 




FIGURE 5.2. 3-4-5 interpolation polynomial and its derivatives. 
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the desired motion in as short a time T as possible. Note, however, that 
this time cannot be given an arbitrarily small value, for we must respect 
the motor specifications on maximum velocity and maximum torque, the 
latter being the subject of Chapter 6. In order to ease the discussion, we 
limit ourselves to specifications of maximum joint velocity and acceleration 
rather than maximum torque. From the form of function 9j{t) of eq.(5.6a), 
it is apparent that this function takes on extreme values at points corre- 
sponding to those at which the normal polynomial attains its extrema. In 
order to find the values of t at which the first and second derivatives of s(t) 
attain maximum values, we need to zero its second and third derivatives. 
These derivatives are displayed below: 



s'{t) = 30 V — 60t® + 30t^ 


(5.14a) 


s"(t) = 120V - 180t 2 -g 60 t 


(5.14b) 


'"(t) = 360 V - 360T + 60 


(5.14c) 



from which it is apparent that the second derivative vanishes at the two 
ends of the interval 0 < t < 1. Additionally, the same derivative vanishes 
at the midpoint of the same interval, i.e., at t = 1/2. Hence, the maximum 
value of s'(t), i® readily found as 



s 



/ 

max 




15 

y 



(5.15) 



and hence, the maximum value of the /th joint rate takes on the value 






max 



8T 



(5.16) 



which becomes negative, and hence, a local minimum, if the difference in 
the numerator is negative. The values of t at which the second derivative 
attains its extreme values are likewise determined. The third derivative 
vanishes at two intermediate points ti and T 2 of the interval 0 < t < 1, 
namely, at 



n,2 



2 6 



and hence, the maximum value of s'\r) is readily found as 



s 



// 

max 



1 

2 



6 



IOa/3 

3 



while the minimum is given as 



s 



// 

min 



1 

2 



V3 

6 



IOa/3 

3 



(5.17) 



(5.18) 



(5.19) 
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Therefore, the maximum value of the joint acceleration is as shown below: 



10 V3 {e^ - Oj) 

3 T2 



Likewise, 



Ca. = «'"(0) = «"'(!) = 60 



and hence. 



ih 



60 



- 0 ^ 



(5.20) 



(5.21) 



Thus, eqs.(5.16) and (5.20) allow us to determine T for each joint so that 
the joint rates and accelerations he within the allowed limits. Obviously, 
since the motors of different joints are different, the minimum values of T 
allowed by the joints will be, in general, different. Of those various values 
of T, we will, of course, choose the largest one. 



5.3.2 A 4-5-6-? Interpolating Polynomial 

Now, from eq.(5.14c), it is apparent that the third derivative of the normal 
polynomial does not vanish at the end points of the interval of interest. This 
implies that the third time derivative of 9j{t), also known as the joint jerk, 
does not vanish at those ends either. It is desirable to have this derivative 
as smooth as the first two, but this requires us to increase the order of the 
normal pol}momial. In order to attain the desired smoothness, we will then 



impose two more conditions, namely, 

s'"(0) = 0, s'"(l) = 0 (5.22) 

We now have eight conditions on the normal polynomial, which means 
that the polynomial degree should be increased to seven, namely, 

s(t) = aP + bP + cP + dP + eP + fp + gr + h (5.23a) 

whose derivatives are readily determined as shown below: 

s'{t) = lap + GhP + 5ct*^ + AdP + 3er^ + 2 /t + g (5.23b) 
.2' {t) = A2aP + 2,{)hP + 20cP + 12dT^ + 6er + 2/ (5.23c) 

s'" (P = 210aT‘^ + I20bp + 60ct^ + 2Adr + 6e (5.23d) 



The first three conditions of eq.(5.7) and the first condition of eq.(5.22) 
readily lead to 



e=f=g=h=0 



(5.24) 
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Furthermore, the last three conditions of eq.(5.7) and the second condition 
of eq.(5.22) lead to four linear equations in four unknowns, namely. 



a-\-b-\-c-\-d = 1 (5.25a) 

7a + 66 + 5c+4d = 0 (5.25b) 

42a + 306 + 20c+12d = 0 (5.25c) 

210a + 1206+ 60c + 24d = 0 (5.25d) 

and hence, we obtain the solution 

a =-20, 6 = 70, c=-84, d = 35 (5.26) 

the desired polynomial thus being 

s(t) = -20t^ + 70t® -84t® +35t-^ (5.27) 



which is a ^-5-6-7 polynomial. This polynomial and its first three deriva- 
tives, normalized to fall within the range ( — 1, 1), are plotted in Fig. 5.3. 
Note that the 4-5-6-7 polynomial is similar to that of Fig. 5.2, except that 
the third derivative of the former vanishes at the extremes of the interval of 
interest. As we will presently show, this smoothness has been obtained at 
the expense of higher maximum values of the first and second derivatives. 

We now determine the maximum values of the velocity and acceleration 
produced with this motion. To this end, we display below the first three 




FIGURE 5.3. 4-5-6-7 interpolating polynomial and its derivatives. 
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derivatives, namely, 

s'{t) = - 140 t® + 420 t® - 420 t-^ + HOrg ( 5 . 28 a) 

s"(t) = - 840 t® + 2100 t-^ - 1680 t 3 + 420 t 2 ( 5 . 28 b) 

s"'(r) = - 4200 t-^ + 8400 t 3 - 5040 t 2 + 840 t ( 5 . 28 c) 



The first derivative attains its extreme values at points where the second 
derivative vanishes. Upon zeroing the latter, we obtain 

t 2 (- 2 t 3 + 5 t 2 - 4 t + 1 ) =0 ( 5 . 29 ) 



which clearly contains a double root at t = 0. Moreover, the cubic polyno- 
mial in the parentheses above admits one real root, namely, t = 1/2, which 
yields the maximum value of s'(t), i.e.. 



s 



/ 

max 




35 

16 



( 5 . 30 ) 



whence the maximum value of the /th joint rate is found as 






max 



16 T 



( 5 . 31 ) 



Likewise, the points of maximum joint acceleration are found upon zeroing 
the third derivative of s(t), namely, 

s'"(r) = - 4200 t-^ + 8400 t 3 - + 840 t = 0 ( 5 . 32 ) 



or 



t(t — 1 )( 5 t^ — 5 t + 1 ) = 0 ( 5 . 33 ) 

which yields, in addition to the two end points, two intermediate extreme 
points, namely. 



1 ^ Vs 
^^’^ = 2^To 

and hence, the maximum value of acceleration is found to be 

84 a/5 



s"(n) 



25 



( 5 . 34 ) 



( 5 . 35 ) 



the minimum occurring at t = Tg, with = — s(/ax- The maximum value 
of the jth joint acceleration is thus 

(^Oj )max 



84 a /5 f ef -ef 



25 



J^2 



( 5 . 36 ) 
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which becomes a minimum if the difference in the numerator is negative. 
Likewise, the zeroing of the fourth derivative leads to 

-20t3 + 30t2 - 12t + 1 = 0 



whose three roots are 

1 - 

= 2 

and hence, 



s 



/// 

max 



i± vWs 

2 



1 

= M 



42, 



1 + 

2 



s"'(0.5) 
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i.e., 

1 05 

max{|s"'(r)|} = — (5.37) 

T 2 

As in the case of the fifth-order polynomial, it is possible to use the 
foregoing relations to determine the minimum time T during which it is 
possible to perform a given PPO while observing the physical limitations 
of the motors. 



5.4 Cycloidal Motion 



An alternative motion that produces zero velocity and acceleration at the 
ends of a finite interval is the cycloidal motion. In normal form, this motion 
is given by 



s(t) = t sin 27TT 

^ ’ 27T 


(5.38a) 


its derivatives being readily derived as 




s' ir) = 1 — cos 2tit 


(5.38b) 


s" ( t) = 2ti sin 2tit 


(5.38c) 


s"'{t) = 47r^COs27TT 


(5.38d) 



The cycloidal motion and its first three time-derivatives, normalized to 
fall within the range ( — 1, 1), are shown in Fig. 5.4. Note that while this 
motion, indeed, has zero velocity and acceleration at the ends of the interval 
0 < T < 1, its jerk is nonzero at these points and hence, exhibits jump 
discontinuities at the ends of that interval. 

When implementing the cycloidal motion in PPO, we have, for the jth 
joint, 

9j{t) = + {9^ - (5.39a) 

C(d = ^ j, ^ s'{t) (5.39b) 

0F 0l 



(5.39c) 
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FIGURE 5.4. The normal cycloidal motion and its time derivatives. 



Moreover, as the reader can readily verify, under the assumption that 9^ > 
9j, this motion attains its maximum velocity at the center of the interval, 
i.e., at T = 0.5, the maximum being 

= s'(0-5) = 2 



and hence, 

-e!^) (5.40a) 

Likewise, the jth joint acceleration attains its maximum and minimum 
values at T = 0.25 and t = 0.75, respectively, i.e.. 



Smax = s"(0.25) = s"(0.75) = 27 t (5.40b) 

and hence, 

(0,)max = - e!^), = -p(0f - 0j) (5.40c) 

Moreover, s"'{t) attains its extrema at the ends of the interval, i.e., 

4'ax = «"'(0) = «'"(!) = 4^^ (5.41) 

and hence, 

47T^ 

(e,U. = j;^{ef-9j) (5.42) 

Thus, if motion is constrained by the maximum speed delivered by the 
motors, the minimum time Tj for the jth joint to produce the given PPO 
can be readily determined from eq. (5.40a) as 



T, 



2{ef- 






(5.43) 
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and hence, the minimum time in which the operation can take place can 
be readily found as 

f ^ 

Tmin = 2 max < -4— — ^ \ (5.44) 

^ y {(^j)max J 

If joint- acceleration constraints are imposed, then a similar procedure can 
be followed to find the minimum time in which the operation can be real- 
ized. As a matter of fact, rather than maximum joint accelerations, maxi- 
mum joint torques are to be respected. How to determine these torques is 
studied in detail in Chapter 6. 



5.5 Trajectories with Via Poses 

The polynomial trajectories discussed above do not allow the specifica- 
tion of intermediate Cartesian poses of the EE. All they guarantee is that 
the Cartesian trajectories prescribed at the initial and final instants are 
met. One way of verifying the feasibility of the Cartesian trajectories thus 
synthesized was outlined above and consists of using a graphics system, 
preferably with animation capabilities, to produce an animated rendering 
of the robot motion, thereby allowing for verification of collisions. If the 
latter occur, we can either try alternative branches of the inverse kine- 
matics solutions computed at the end poses or modify the trajectory so 
as to eliminate collisions. We discuss below the second approach. This is 
done with what are called via poses, i.e., poses of the EE in the Cartesian 
space that lie between the initial and the final poses, and are determined 
so as to avoid collisions. For example, if upon approaching the final pose 
of the PPO, the manipulator is detected to interfere with the surface on 
which the workpiece is to be placed, a via pose is selected close to the final 
point so that at this pose, the workpiece is far enough from the surface. 
From inverse kinematics, values of the joint variables can be determined 
that correspond to the aforementioned via poses. These values can now 
be regarded as points on the joint-space trajectory and are hence called 
via points. Obviously, upon plotting each joint variable vs. time, via points 
appear as points on those plots as well. 

The introduction of via points in the joint-space trajectories amounts to 
an increase in the number of conditions to be satisfied by the desired tra- 
jectory. For example, in the case of the polynomial trajectory synthesized 
for continuity up to second derivatives, we can introduce two via points by 
requiring that 



s(n) = si, s{t2) = S2 (5.45) 

where ti, T 2 , si, and «2 depend on the via poses prescribed and the instants 
at which these poses are desired to occur. Hence, si and «2 differ from joint 
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to joint, although the occurrence instants ti and T2 are the same for all 
joints. Thus, we will have to determine one normal polynomial for each 
joint. Furthermore, the ordinate values si and «2 of the normal polynomial 
at via points are determined from the corresponding values of the joint 
variable determined, in turn, from given via poses through inverse kine- 
matics. Once the via values of the joint variables are known, the ordinate 
values of the via points of the normal polynomial are found from eq.(5.6a). 
Since we have now eight conditions to satisfy, namely, the six conditions 
(5.7) plus the two conditions (5.45), we need a seventh-order polynomial. 



i.e., 

s(t) = a,T^ + 6t® + CT® + dr'^ + er® + /t^ + (jt + h (5.46) 

Again, the first three conditions of eq.(5.7) lead to the vanishing of the 
last three coefficients, i.e., 

f = g = h = 0 (5.47) 

Further, the five remaining conditions are now introduced, which leads to 
a system of five linear equations in five unknowns, namely, 

(2T6 TcTdTe = 1 (5.48a) 

7a + 66 + 5c + 4d + 3e = 0 (5.48b) 

42a + 306 + 20c + 12d + 6e = 0 (5.48c) 

rla + rfb + rfc + T^d + rfe = Si (5.48d) 

a + t |6 + t|c + T 2 d + t|c = «2 (5.48e) 

where ti, T 2 , si, and «2 are all data. For example, if the via poses occur at 
10% and 90% of T, we have 

Ti = l/10, T2=9/10 (5.481) 

the polynomial coefficients being found as 

a = 100(12286+ 12500S1 - 12500s2)/729 (5.49a) 

6 = 100(-38001 - 48750S1 + 38750s2)/729 (5.49b) 

c = (1344358+ 2375000S1 - 1375000s2)/243 (5.49c) 

d = (-1582435 - 4625000S1 + 1625000s2)/729 (5.49d) 

e = 10(12159+ 112500S1 - 12500s2)/729 (5.49e) 



The shape of each joint trajectory thus depends on the values of si and «2 
found from eq. (5.6a) for that trajectory. 



5.6 Synthesis of PPO Using Cubic Splines 

When the number of via poses increases, the foregoing approach may be- 
come impractical, or even unreliable. Indeed, forcing a trajectory to pass 
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through a number of via points and meet endpoint conditions is equivalent 
to interpolation. We have seen that an increase in the number of condi- 
tions to be met by the normal polynomial amounts to an increase in the 
degree of this polynomial. Now, finding the coefficients of the interpolating 
polynomial requires solving a system of linear equations. As we saw in Sec- 
tion 4.9, the computed solution, when solving a system of linear equations, 
is corrupted with a relative roundoff error that is roughly equal to the rel- 
ative roundoff error of the data multiplied by an amplification factor that 
is known as the condition number of the system matrix. As we increase 
the order of the interpolating polynomial, the associated condition num- 
ber rapidly increases, a fact that numerical analysts discovered some time 
ago (Kahaner et ah, 1989). In order to cope with this problem, orthogo- 
nal polynomials, such as those bearing the names of Chehyshev, Laguerre, 
Legendre, and so on, have been proposed. While orthogonal polynomials 
alleviate the problem of a large condition number, they do this only up to 
a certain extent. As an alternative to higher-order polynomials, spline func- 
tions have been found to offer more robust interpolation schemes (Dierckx, 
1993). Spline functions, or splines, for brevity, are piecewise polynomials 
with continuity properties imposed at the supporting points. The latter are 
those points at which two neighboring polynomials join. 

The attractive feature of splines is that they are defined as a set of 
rather lower-degree polynomials joined at a number of supporting points. 
Moreover, the matrices that arise from an interpolation problem associated 
with a spline function are such that their condition number is only slightly 
dependent on the number of supporting points, and hence, splines offer 
the possibility of interpolating over a virtually unlimited number of points 
without producing serious numerical conditioning problems. 

Below we expand on periodic cubic splines, for these will be shown to be 
specially suited for path planning in robotics. 

A cubic spline function s(x) connecting N points Pk {xk, yk), for k = 
1,2, . . . , N , is a function defined piecewise by W — 1 cubic polynomials 
joined at the points Pk, such that s{xk) = yk- Furthermore, the spline 
function thus defined is twice differentiable everywhere in xi < x < x^r. 
Hence, cubic splines are said to be functions, i.e., to have continuous 
derivatives up to the second order. 

Cubic splines are optimal in the sense that they minimize a functional, 
i.e., an integral defined as 

fT 

F= s"'^ (x) dx 

Jo 

subject to the constraints 

s(xk) = yk, k=l,...,N 

where Xk and yk are given. The aforementioned optimality property has 
a simple kinematic interpretation: Among all functions defining a motion 
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so that the plot of this function passes through a set of points Pi(xi, si), 
P 2 {x 2 , S 2 ), . . Pn{xn, sn) in the x-s plane, the cubic spline is the one 
containing the minimum acceleration magnitude . In fact, F, as given above, 
is the square of the Euclidean norm (Halmos, 1974) of s"(x), i.e., F turns 
out to be a measure of the magnitude of the acceleration of a displacement 
program given by s(x), if we interpret s as displacement and x as time. 

Let Pk{xk, Vk) and yk+i) be two consecutive supporting 

points. The A:th cubic polynomial Sfc(x) between those points is assumed 
to be given by 

Sk{x) = Ak{x - Xkf + Bk{x - Xkf +Ck{x - Xk) + Dk (5.50a) 

for Xk < X < Xk+i- Thus, for the spline s(x), 4(N — 1) coefficients Ak, Bk, 
Ck, Dk, for k = 1, . . . , N — 1, are to be determined. These coefficients will 
be computed presently in terms of the given function values and 

the second derivatives of the spline at the supporting points, {s'^Xk)}^ , 
as explained below: 

We will need the first and second derivatives of Sfc(x) as given above, 
namely. 



s'k{x) = 3Ak{x - Xkf + 2Bk{x - Xk) + Ck 


(5.50b) 


Sk{x) = 6Ak(x - Xk) + 2Bk 


(5.50c) 


whence the relations below follow immediately: 




Pk — -^Sk 


(5.5fa) 


Ck = s'k 


(5.5fb) 


Dk = Sk 


(5.5fc) 


where we have used the abbreviations 




Sk = s{xk), s'k = s'{xk), s'k = s"{xk) 


(5.52) 


Furthermore, let 




22^Xk = Xk^\ Xk 


(5.53) 


From the above relations, we have expressions for coefficients Bk and Dk in 
terms of and Sk, respectively, but the expression for Ck is given in terms 


of s'l,. What we would like to have are similar expressions 


for Ak and Ck, 


i.e., in terms of Sk and s((. The relations sought will be found by imposing 
the continuity conditions on the spline function and its first and second 
derivatives with respect to x at the supporting points. These conditions 


are, then, for A: = 1 , 2, . . . , W — I , 






(5.54a) 


s'k{xk+l) = Sfc+1 


(5.54b) 


s'kixk+l) = Sfc+1 


(5.54c) 
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Upon substituting as given by eq. (5.50c), into eq. (5.54c), we 

obtain 

GAfcAxfc + 2Bk = 2Bj~+i 

but from eq. (5.51a), we have already an expression for Bk, and hence, one 
for Bk+i as well. Substituting these two expressions in the above equation, 
we obtain an expression for Ak, namely, 

'-'5. - >0 

Furthermore, if we substitute Sk{xk+i), as given by eq. (5.50a), into eq.(5.54a), 
we obtain 



Ak{A.Xk)^ + Bfc(Axfc)^ + CkA.Xk + Dk — Sfc+i 



But we already have values for Ak and Bk from eqs.(5.54d) and (5.51a), re- 
spectively. Upon substituting these values in the foregoing equation, we ob- 
tain the desired expression for Ck in terms of function and second-derivative 
values, i.e., 

C’fc = ^ Axfc (s"+i + 2s'') (5.54e) 

In summary, then, we now have expressions for all four coefficients of the 
A:th polynomial in terms of function and second-derivative values at the 
supporting points, namely. 



with 





(5.55a) 




(5.55b) 




(5.55c) 


Sk 


(5.55d) 



Asfc = Sfc+i - Sk (5.55e) 

Furthermore, from the requirement of continuity in the first derivative, 
eq.(5.54b), after substitution of eq. (5.50b), one obtains 

3iAk{A.Xk)‘^ + -f Ck = Ck+i 

or if we shift to the previous polynomials. 



34lfc_i(Axfc_i)^ -f 2Bk-iA.Xk-i + Ck-i — Ck 
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Now, if we substitute expressions (5.55a-c) in the above equation, a linear 
system of fV — 2 simultaneous equations for the N unknowns is 

obtained, namely. 



= 4 ^ 

\Axk 



+ 2(Axfc_i + Axk)s'l + (Axfc_i)sfc_i 



Asfc-i 

Axfc_i 



for k = 2, . . . , N — I 



(5.56) 



Further, let s be the A-dimensional vector whose A:th component is Sk, 
with vector s" being defined likewise, i.e., 

s= [si,...,SArf , s"= (5.57) 

The relationship between s and s" of eq.(5.56) can then be written in 
vector form as 

As" = 6Cs (5.58a) 



where A and C are {N — 2) x N matrices defined as: 





■ Ol 


2ai,2 


0.2 


0 


0 


0 - 






0 


02 


202,3 


03 


0 


0 




A = 


0 


0 




Oat/// 


‘lo.jSl"' O-N" 


0 


(5.58b) 




. 0 


0 


0 




CXjSf" 2oijV",A^' 


Oat/ . 




and 


-/3i 


— 1^1,2 


/?2 


0 


0 


0 ■ 






0 


/?2 


-/?2,3 


p3 


0 


0 




C = 


0 


0 




pN'" 


— pN'",N" pN" 


0 


(5.58c) 




. 0 


0 


0 




pN" —pN",N' 


pN' ■ 




while for i,j, k = 1, . . 


.,N- 


1, 














Ok = 


Axfc, 


^'i,j — 




(5.58d) 








Pk = 


= 1/ofc, 


(^i,j — f^i f^j 




(5.58e) 


and 




N' = N 


-1, 


N" = 


; TV - 2, N''' = N 


-3 


(5.58f) 



Thus, two additional equations are needed to render eq. (5.58a) a deter- 
mined system. The additional equations are derived, in turn, depending 
upon the class of functions one is dealing with, which thus gives rise to 
various types of splines. For example, if s" and s'^ are defined as zero. 
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then one obtains natural cubic splines, the name arising by an analogy 
with beam analysis. Indeed, in beam theory, the boundary conditions of 
a simply-supported beam establish the vanishing of the bending moments 
at the ends. Prom beam theory, moreover, the bending moment is propor- 
tional to the second derivative of the elastica, or neutral axis, of the beam 
with respect to the abscissa along the beam axis in the undeformed con- 
figuration. In this case, vector s" becomes of dimension N — 2, and hence, 
matrix A becomes, correspondingly, of {N — 2) x {N — 2), namely. 



" 2o;i 2 ^-2 d ' ' ' 0 " 

02 2 o 2^3 O3 • • • 0 

A= : : (5.59) 

0 ... otv'" 2o!7V"',Af" ow" 

0 0 • • • otv" 2ctAf",Af' - 



On the other hand, if one is interested in periodic functions, which is often 
the case when synthesizing pick-and-place motions, then the conditions 
Si = sn, s) = s)y, s" = s)(r are imposed, thereby producing periodic cubic 
splines. The last of these conditions is used to eliminate one unknown in 
eq. (5.58a), while the second condition, namely the continuity of the first 
derivative, is used to add an equation. We have, then, 

s'l = s)v (5.60) 

which can be written, using eq. (5.54b), as 

s'l = s)v_;^(xAr) (5.61) 

Upon substituting s)y_]^(xAr), as given by eq. (5.50b), into the above equa- 
tion, we obtain 

s) = 3 Atv— + 2 B 7 V— lAx^v — 1 + Utv — 1 (5.62) 

Now we use eqs.(5.55a-c) and simplify the expression thus resulting, which 
leads to 

2{Axi+AxN-i)s'l + AxiS2+AxN-is%_.^ = 6 f '] (5.63) 

\Axi Axn-1 ) 

thereby obtaining the last equation required to solve the system of equa- 
tions given by eqs.(5.58a-c). We thus have [A — 1) independent equations 
to solve for [A — 1) unknowns, namely, s((, for A: = 1, . . . , W — 1, s”-^ being 
equal to s". Expressions for matrices A and C, as applicable to periodic 
cubic splines, are given in eqs.(9.59a & b). 

While we focused in the above discussion on cubic splines, other types 
of splines could have been used. For example, Thompson and Patel (1987) 
used B-splines in robotics trajectory planning. 
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FIGURE 5.5. Errors in the approximation of a 4-5-G-7 polynomial with a natural 
cubic spline, using four supporting points. 



Example 5.6.1 (Approximation of a 4-5-6-T polynomial with a cu- 
bic spline) Find the cubic spline that interpolates the 5- 6-7 polynomial 
of Fig. 5.3 with fV + 1 equally- spaced supporting points and plot the inter- 
polation error for N = 2> and N = 10. 

Solution: Let us use a natural spline, in which case the second derivative at 
the end points vanishes, with vector s" thus losing two components. That is, 
we now have only IV— 1 unknowns { s'l to determine. Correspondingly, 
matrix A then loses its first and last columns and hence, becomes a square 
{N — 1) X {N — 1) matrix. Moreover, 

Axfc = ^, k=l,...,N 

and matrices A and C become, correspondingly, 

1 0 ••• O' 

4 1 ••• 0 

..141 
0 ••• 1 4 



1 

N 
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FIGURE 5.6. Errors in the approximation of a 4-S-6-7 polynomial with a natural 
cubic spline, using eleven supporting points. 



and 



' 1-2 1 0 
0 1-21 



C = N 



0 O' 
0 0 



0 0 •• • 1 -2 1 0 
0 0 0 •• • 1 -2 1 



the vector of second derivatives at the supporting points, s", then being 
readily obtained as 

s" = 6A-^Cs 



With the values of the second derivatives at the supporting points known, 
the calculation of the spline coefficients Ak, Bk, Ck, and Dk, for k = 
1, . . . , N, is now straightforward. Let the interpolation error, e(x), be de- 
fined as e(x) = s(x) —p(x), where s(x) is the interpolating spline and p(x) 
is the given polynomial. This error and its derivatives e'(x), e"(x), and 
e"'[x) are plotted in Figs. 5.5 and 5.6 for IV = 3 and N = 10, respectively. 
What we observe is an increase of more than one order of magnitude in the 
error as we increase the order of the derivative by one. Thus, the order of 
magnitude of acceleration errors is usually higher than two orders of mag- 
nitude above the displacement errors, a fact that should not be overlooked 
in applications. 




6 

Dynamics of Serial Robotic 
Manipulators 



6.1 Introduction 

The main objectives of this chapter are (*) to devise an algorithm for the 
real-time computed torque control and (n) to derive the system of second- 
order ordinary differential equations (ODE) governing the motion of an 
n-axis manipulator. We will focus on serial manipulators, the dynamics 
of a much broader class of robotic mechanical systems, namely, parallel 
manipulators and mobile robots, being the subject of Chapter 10. Moreover, 
we will study mechanical systems with rigid links and rigid joints and 
will put aside systems with flexible elements, which pertain to a more 
specialized realm. 



6.2 Inverse vs. Forward Dynamics 

The two basic problems associated with the dynamics of robotic mechani- 
cal systems, namely, the inverse and the forward problems, are thoroughly 
discussed in this chapter. The relevance of these problems cannot be over- 
stated: the former is essential for the computed-torque control of robotic 
manipulators, while the latter is required for the simulation and the real- 
time feedback control of the same systems. Because the inverse problem 
is purely algebraic, it is conceptually simpler to grasp than the forward 
problem, and hence, the inverse problem will be discussed first. Moreover, 
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the inverse problem is also computationally simpler than the forward prob- 
lem. In the inverse problem, a time-history of either the Cartesian or the 
joint coordinates is given, and from knowledge of these histories and the 
architecture and inertial parameters of the system at hand, the torque 
or force requirements at the different actuated joints are determined as 
time- histories as well. In the forward problem, current values of the joint 
coordinates and their first time-derivatives are known at a given instant, 
the time- histories of the applied torques or forces being also known, along 
with the architecture and the inertial parameters of the manipulator at 
hand. With these data, the values of the joint coordinates and their time- 
derivatives are computed at a later sampling instant by integration of the 
underlying system of nonlinear ordinary differential equations. 

The study of the dynamics of systems of multiple rigid bodies is classical, 
but up until the advent of the computer, it was limited only to theoreti- 
cal results and a reduced number of bodies. First Uicker (1965) and then 
Kahn (1969) produced a method based on the Euler-Lagrange equations 
of mechanical systems of rigid bodies that they used to simulate the dy- 
namical behavior of such systems. A breakthrough in the development of 
algorithms for dynamics computations was reported by Luh et al. (1980), 
who proposed a recursive formulation of multibody dynamics that is appli- 
cable to systems with serial kinematic chains. This formulation, based on 
the Newton-Euler equations of rigid bodies, allowed the calculation of the 
joint torques of a six-revolute manipulator with only 800 multiplications 
and 595 additions, a tremendous gain if we consider that the straightfor- 
ward calculation of the Euler-Lagrange equations for the same type of ma- 
nipulator involves 66,271 multiplications and 51,548 additions, as pointed 
out by Hollerbach (1980). In the foregoing reference, a recursive derivation 
of the Euler-Lagrange equations was proposed, whereby the computational 
complexity was reduced to only 2,195 multiplications and 1,719 additions. 

The foregoing results provoked a discussion on the merits and demerits 
of each of the Euler-Lagrange and the Newton-Euler formulations. Silver 
(1982) pointed out that since both formulations are equivalent, they should 
lead to the same computational complexity. In fact. Silver showed how to 
derive the Euler-Lagrange equations from the Newton-Euler formulation 
by following an approach first introduced by Kane (1961) in connection 
with nonholonomic systems. Kane and Levinson (1983) then showed how 
Kane’s equations can be applied to particular robotic manipulators and 
arrived at lower computational complexities. They applied the said equa- 
tions to the Stanford Arm (Paul, 1981) and computed its inverse dynamics 
with 646 multiplications and 394 additions. Thereafter, Khalil et al. (1986) 
proposed a condensed recursive Newton-Euler method that reduced the 
computational complexity to 538 multiplications and 478 additions, for ar- 
bitrary architectures. Further developments in this area were reported by 
Balafoutis and Patel (1991), who showed that the underlying computa- 
tional complexity can be reduced to 489 multiplications and 420 additions 
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for the most general case of a six- revolute manipulator, i.e., without ex- 
ploiting particular features of the manipulator geometry. Balafoutis and 
Patel based their algorithm on tensor analysis, whereby tensor identities 
are exploited to their fullest extent in order to reduce the number of op- 
erations involved. Li and Sankar (1992), in turn, reported further savings 
that allowed them to bring down those numbers to 459 multiplications and 
390 additions. 

In this chapter, the inverse dynamics problem is solved with the well- 
known recursive Newton-Euler algorithm, while the forward dynamics prob- 
lem is handled with a novel approach, based on the reciprocity relations 
between the constraint wrenches and the feasible twists of a manipulator. 
This technique is developed with the aid of a modeling tool known as the 
natural orthogonal complement, thoroughly discussed in Section 6.5. 

Throughout the chapter, we will follow a multibody system approach, 
which requires a review of the underlying fundamentals. 



6.3 Fundamentals of Multibody System Dynamics 



6.3.1 On Nomenclature and Basic Definitions 



We consider here a mechanical system composed of r rigid bodies and 
denote by Mj the 6x6 inertia dyad — see Section 3.8 — of the *th body. 
Moreover, we let Wj, already introduced in eq.(3.145), be the 6x6 angular- 
velocity dyad of the same body. As pertaining to the case at hand, the said 
matrices are displayed below: 



M,- = 



li O 


W,- = 


'a 


o 


O mjl 


7 ^ ^ t 


o 


o 



i = 1, 



( 6 . 1 ) 



where 1 and O denote the 3x3 identity and zero matrices, respectively, 
while Vli and Ij are the angular- velocity and the inertia matrices of the 
*th body, this last being defined with respect to the mass center Q of this 
body. Moreover, the mass of this body is denoted by m*, whereas Cj and 
Cj denote the position and the velocity vectors of Ci. Furthermore, let tj 
denote the twist of the same body, the latter being defined in terms of 
the angular velocity vector u>i, the vector of fij, and the velocity of Q. 
The 6-dimensional momentum screw /j.^ is defined likewise. Furthermore, 
wF and w'f are defined as the working wrench and the nonworking con- 
straint wrench exerted on the *th body by its neighbors, in which forces are 
assumed to be applied at Ci. We thus have, for * = 1, . . . , r. 



ti 




mjCj ’ 



.w 



w 

nfi 

fW 



nf 

ff 



( 6 . 2 ) 



where superscripted rij and fj stand, respectively, for the moment and the 
force acting on the *th body, the force being applied at the mass center Q. 
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Thus, whereas 'wY accounts for forces and moments exerted by both the 
environment and the actuators, including driving forces as well as dissipa- 
tive effects, whose sole function is to keep the links together, accounts 
for those forces and moments exerted by the neighboring links, which do 
not produce any mechanical work. Therefore, friction wrenches applied by 
the (* — l)st and the (* + l)st links onto the *th link are not included in 
rather, they are included in 'wY ■ 

Clearly, from the definitions of Mj, /i.^, and tj, we have 

/LXj = Mjtj (6.3) 

Moreover, from eq.(3.148), 

/itj = Mjtj + Wj/Lx^ = Mjtj + WjMjtj (6.4) 

We now recall the Newton-Euler equations for a rigid body, namely, 

Ijthj = -ijJi X IjWj + vlY + nf (6.5a) 

mjCj = iY + iY (6.5b) 

which can be written in compact form using the foregoing 6-dimensional 
twist and wrench arrays as well as the 6x6 inertia and angular- velocity 
dyads. We thus obtain the Newton-Euler equations of the *th body in the 
form 

Mjtj = -WjMjtj + wY + wf (6.5c) 



6.3.2 The Euler- Lagrange Equations of Serial 
Manipulators 



The Euler-Lagrange dynamical equations of a mechanical system are now 
recalled, as pertaining to serial manipulators. Thus, the mechanical system 
at hand has n degrees of freedom, its n independent generalized coordinates 
being the n joint variables, which are stored in the n-dimensional vector 9. 
We thus have 



d fdT\ dT 
dt \de ) 99 



( 6 . 6 ) 



where T is a scalar function denoting the kinetic energy of the system and 0 
is the n-dimensional vector of generalized force. If some forces on the right- 
hand side stem from a potential V, we can, then decompose 4> into two 
parts, 4>p and the former arising from V and termed the conservative 
force of the system; the latter is the nonconservative force That is. 



dV 






99 



(6.7) 



the above Euler-Lagrange equations thus becoming 



d f 9L\ 9L 

dt \d9 ) 99 



0 - 



( 6 . 8 ) 
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where L is the Lagrangian of the system, defined as 

L = T -V (6.9) 



Moreover, the kinetic energy of the system is simply the sum of the kinetic 
energies of all the r links. Recalling eq.(3.150), which gives the kinetic 
energy of a rigid body in terms of 6-dimensional arrays, one has 

r ’^1 

T = = (6.10) 

1 1 ^ 



whereas the vector of nonconservative generalized forces is given by 



_ dn^ dA 

80 80 



( 6 . 11 ) 



in which 11'^ and A denote the power supplied to the system and the 
Rayleigh dissipation function, or for brevity, the dissipation function of the 
system. 

The first of these items is discussed below; the latter is only outlined in 
this section but is discussed extensively in Section 6.8. First, the wrench 
is decomposed into two parts, and wf, the former being the wrench 
supplied by the actuators and the latter being the wrench that arises from 
viscous and Coulomb friction, the gravity wrench being not needed here 
because gravity effects are considered in the potential V{0). We thus call 
wf the active wrench and the dissipative wrench. Here, the wrenches 
supplied by the actuators are assumed to be prescribed functions of time. 
Moreover, these wrenches are supplied by single-dof actuators in the form of 
forces along a line of action or moments in a given direction, both line and 
direction being fixed to the two bodies that are coupled by an active joint. 
Hence, the actuator-supplied wrenches are dependent on the posture of the 
manipulator as well, but not on its twist. That is, the actuator wrenches are 
functions of both the vector of generalized coordinates, or joint variables, 
and time, but not of the generalized speeds, or joint-rates. Forces dependent 
on the latter to be considered here are assumed to be all dissipative. As a 
consequence, they can be readily incorporated into the mathematical model 
at hand via the dissipation function, to be discussed in Section 6.8. Note 
that feedback control schemes require actuator forces that are functions 
not only of the generalized coordinates, but also of the generalized speeds. 
These forces or moments are most easily incorporated into the underlying 
mathematical model, once this model is derived in the state- variable space, 
i.e., in the space of generalized coordinates and generalized speeds. 

Thus, the power supplied to the *th link, lif, is readily computed as 

(6.12a) 



Similar to the kinetic energy, then, the power supplied to the overall 
system is simply the sum of the individual powers supplied to each link. 




216 



6. Dynamics of Serial Robotic Manipulators 



and expressed as in eq.(6.12a), i.e., 

r 

(6.12b) 

1 

Further definitions are now introduced. These are the 6n-dimensional 
vectors of manipulator twist, t; manipulator momentum, fX] manipulator- 
constraint wrench, w'^; manipulator active wrench, and manipulator 
dissipative wrench, w^. Additionally, the 6n x 6n matrices of manipulator- 
mass, M, and manipulator angular velocity, W, are also introduced below: 



Ml 

Mn 



(6.13a) 



w“ =1 : I , w'" =1 : I , w"' = I : I (6.13b) 

M = diag(Mi, ..., M„), W = diag(Wi, . . . , W„ ) (6.13c) 

It is now apparent that, from definitions (6.13b & 6.13c) and relation 
(6.3), we have 



'wf 




'wf 


..rD 


'wf ■ 




, W = 




, w = 




w'c 

L n J 




L n J 




1 

Os 

1 



= Mt (6.14) 

Moreover, from definitions (6.1) and (6.2), 

/it = Mt + WMt (6.15) 

With the foregoing definitions, then, the kinetic energy of the manipulator 
takes on a simple form, namely, 

T = -t^Mt = 2^^/^ (6.16) 

which is a quadratic form in the system twist. Since the twist, on the other 

hand, is a linear function of the vector 9 of joint rates, the kinetic energy 

turns out to be a quadratic form in the vector of joint rates. Moreover, we 
will assume that this form is homogeneous in 9, i.e., 

T=^9^I{9)9 (6.17) 

Notice that the above assumption implies that the base of the robot is 
fixed to an inertial base, and hence, when all joints are locked, the kinetic 
energy of the robot vanishes, which would not be the case if, for example, 
the robot were mounted on the International Space Station. If this were the 
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case, then the kinetic energy would not vanish even if all robot joints were 
locked, which means that the foregoing kinetic-energy expression would 
include a linear term in 9 and a term independent of the joint-rates. In any 
event, it is apparent that 



1(0) = ^(T) (6.18) 

89 

which means that the nxn generalized inertia matrix is the Hessian matrix 
of the kinetic energy with respect to the vector of generalized speed. 
Furthermore, the Euler-Lagrange equations can be written in the form 



d rdT\ 8T dV 

~dt 



(6.19a) 



Now, from the form of T given in eq.(6.17), the partial derivatives appearing 
in the foregoing equation take the forms derived below: 



8T 

89 



1(9)9 



and hence, 

= 1(9)9 + i(9, 9)9 (6.19b) 

Moreover, in order to calculate the second term of the left-hand side of 
eq.(6.19a), we express the kinetic energy in the form 

T=ip(0,0)^0 (6.19c) 



d f8T 
dt \89 



where p(0, 9) is the generalized momentum of the manipulator, defined as 



Hence, 



or 



p(0,0) 


^1(0) 
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(6.19d) 
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(6.19e) 
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8T 1 
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~8(19)~ 

89 


9 


(6.19f) 



the Euler-Lagrange equations thus taking on the alternative form 



1(9)9 + 1(9, 9)9 



1 

2 



. n T 



8(19) 

89 



8V 



0 - 



( 6 . 20 ) 
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P(x,y) 




FIGURE 6.1. A planar manipulator. 

Example 6 . 3.1 (Euler-Lagrange equations of a planar manipula- 
tor) Consider the manipulator of Fig. 6 . 1 , with links designed so that their 
mass centers, Ci, C2, and C^, are located at the midpoints of segments 
O1O2, O2O37 o-nd O^P, respectively. Moreover, the ith link has a mass mi 
and a centroidal moment of inertia in a direction normal to the plane of 
motion li, while the joints are actuated by motors delivering torques ti, 
T2, and T3, the lubricant of the joints producing dissipative torques that we 
will neglect in this model. Under the assumption that gravity acts in the 
direction of —Y, find the associated Euler-Lagrange equations. 

Solution: Here we recall the kinematic analysis of Section 4.8 and the def- 
initions introduced therein for the analysis of planar motion. In this light, 
all vectors introduced below are 2-dimensional, the scalar angular velocities 
of the links, evi, for * = 1,2, 3, being 

LVl = 9 l, LV 2 = 0 \ Y 62, LV 2 , = Y 62 + 63 

Moreover, the velocities of the mass centers are 
Cl = i^iEai 

C2 = 0iEai + + 02)Ea2 

C 3 = liiEai + (9i Y 02)Ea2 + Y (I 2 Y ^ 3 )Ea 3 
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the kinetic energy then becoming 

T = +W) 

^ 1 

The squared magnitudes of the mass-center velocities are now computed 
using the expressions derived above. After simplifications, these yield 



||c2 Ip — (£j)\ + “£12(^1 T 201^2 + ^2) T aiCl2 cos 02{p\ + ^\^2) 

IlcalP = a\^\ + £*2(^1 + 20i<?2 + 

+ ”££3(1^1 + ^^2 T 1^3 + 201^2 + 201^3 + 2820^) 

-\-2aitt2 cos 02 I + + £ll£l3 COs(02 + ^^3)(^1 + 1^2 + 

+ 0203 COS 03 { 9 i + ^2 + 20\02 + 01<?3 + 020 ^) 

The kinetic energy of the whole manipulator thus becomes 

T = —{Ill0\ + 2 Ii 20102 + 212302^3 + 122^2 + 2/l30103 + I33S3) 

with coefficients Aj, for * = 1 , 2 , 3 , and j = * to 3 being the distinct entries 
of the 3 x 3 matrix of generalized inertia of the system. These entries are 
given below: 



1 2 / 2 1 2 A 

/ii = /i + /2 + /3 + — miO]^ + m2 ( O]^ + — O 2 + 01 O 2 C 2 j 
+m 3 ^o^ + O 2 + ^££3 + 201 O 2 C 2 + 01 O 3 C 23 + 02 O 3 CJ 
I\2 = /2 + /3 + — m 2 + Oi02C2^ 

/ 2 1 2 A 

+ £££3 I 2o2 + — O3 + 2O1O2C2 + O1O3C23 + 2O2O3C3 I 

I fl 2 \ 

2 ™^ 1 2®3 £££££3023 + ££2££3C3 I 

-I / -I \ 



I13 = h + 2 

122 = -^2 + -^3 + ^£££2 ££2 T £££3 ^££2 T —03 + 

123 =h + ^£££3 ^££1 + ££ 2 ££ 3 C 3 ^ 



02 O 3 C 3 



-^33 = I3 



1 



m 30 g 



4 

where Cj and Cjj stand for cos 0 j and cos( 0 j + 0 j), respectively. From the 
foregoing expressions, it is apparent that the generalized inertia matrix is 
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not a function of <?i, which is only natural, for if the second and third joints 
are locked while leaving the first one free, the whole manipulator becomes 
a single rigid body pivoting about point Oi. Now, the polar moment of 
inertia of a rigid body in planar motion about a fixed point is constant, 
and hence, the first joint variable should not affect the generalized inertia 
matrix. 

Furthermore, the potential energy of the manipulator is computed as the 
sum of the individual link potential energies, i.e.. 



V = —niigai sin 9 \ + ni2g 



ai sin 6i + -02 sin( 6 »i + 62) 



+ni3g 



ai sin 6 »i + a,2 sin( 6 »i + 62) + -03 sin( 6 »i + 62+ O3) 



while the total power delivered to the manipulator takes the form 

n = Ti^i + T2O2 + T3O3 



We now proceed to compute the various terms in eq.( 6 . 20 ). We already 
have 1 ( 0 ), but we do not have, as yet, its time-derivative. However, the 
entries of I are merely the time-derivatives of the entries of I. Prom the 
above expressions for these entries, their time-rates of change are readily 
calculated, namely, 

111 = -m2aia2S202 - m3[2aia2S202 + aia3«23(02 + O 3 ) + a2a3«303] 

112 = ^{-rri2aia2S292 - m3[2aia2S202 + aia3«23(02 + ^ 3 ) + 202035303 ]} 

113 = -^TO3[aia3S23(02 + O 3 ) + O 2 O 3 S 303 ] 

I22 = — m3O2O3S303 

hs = — -’rr 3 O 2 O 3 S 303 

hs = 0 

with Sij defined as sin( 0 j + 6 j)- It should now be apparent that the time- 
rate of change of the generalized inertia matrix is independent of 0 i, as 
one should have expected, for this matrix is independent of 0 i. That is, 
if all joints but the first one are frozen, no matter how fast the first joint 
rotates, the manipulator moves as a single rigid body whose polar moment 
of inertia about Oi, the center of the first joint, is constant. As a matter of 
fact, I33 is constant for the same reason and I33 hence vanishes. We have. 
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then/ 



ie = i 

whose components, tj, for i 



iudi + /i2<^2 + 
il2^1 + h2^2 + 

+ ^23^2 + h3^3_ 

1, 2, 3, are readily calculated as 



(-1 = -[m2aia2S2 + m3ai(2a2S2 + a3S23)]^i^2 - ^303(01523 + 0253)^1^3 
-i[m2aia2S2 + m3ai(2a2S2 + a3S23)]^| - m3a3(aiS23 + a2«3)^2^3 

-^«r3a3(aiS23 + a2S3)^I 

(-2 = -^[^2010252 + m3ai(2a2S2 + a3S23)]^i^2 

-^«r3a3(aiS23 + 0253 )^ 1^3 - m3a2a3S3^2^3 - ^m3a2a3S3^i 

(-3 = -^m3aia3S23^1^3 - ^m3a3(aiS23 + 0253)^1^3 - ^m3a2a3S3^2^3 

The next term in the right-hand side of eq.(6.20) now requires the cal- 
culation of the partial derivatives of vector 10 with respect to the joint 
variables, which are computed below. Let 



djie) _ 
de ~ 

its entries being denoted by //. This matrix, in component form, is given 

by 



r 



0 /ll,2^^1 + -fl2,2^^2 + -fl3,2^^3 
0 Il2,2Sl + 122 , 2(^2 + 123 , 2(^3 
.0 Il3,2(^l + 123,282 + 133,283 



111.381 + 112,382 + 113,383 

112.381 + 122,382 + 123,383 

113.381 + 123,382 + 133 , 383 . 



with the shorthand notation /jj-y, indicating the partial derivative of lij 
with respect to 9k- As the reader can verify, these entries are given as 



// = 0 

I 12 = — [m2aia2S2 + m^{2aia2S2 + aia3S23)]8i 

— -[m2aia2S2 + ms{2aia2S2 + aia3S23)]82 ~ -^'m-saiasS238s 

I'ls = —m2j{aia2jS23 + a2a3S3)8i — -^- 3(0103523 + “ 2 ^ 0 , 20 , 383)92 
— -ms(o,io,sS23 + o,20,^s^)93 



is the Greek letter iota, and denotes a vector; according to our notation, its 
components are ii, 12 , and is. 
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/'i=0 

1 22 = — — [m2aia2S2 + m^{2aia2S2 + a\a?:,S2-s,)]di 

123 = —-^'rns(aiasS 23 + 2020353)01 — 711^0,20,^8^62 

132 = --^7712,0102,82361 

133 = --^7712(0,10,2823 + 020283)61 — -77I2O2O28362 
Now, we define the 3-dimensional vector 7 below: 

r . n T 



7 = 



d(I6>) 

80 



0 



-'771202028262 



its three components, 7*, for * = 1, 2, 3, being 

71 = 0 

72 = -[m20i02S2 + 7712(2010282 + OiO3S23)]0i 

-[m20i02S2 + 7712(2010282 + aiO 3 S 23)]0102 
—771201028226162 

73 = -7713(0103523 + 020282)61 - 7773(0103523 + 20,20,282)6162 

— 7773(0103523 + 020282)6162 — 771202028262 — 77120202826262 

We now turn to the computation of the partial derivatives of the potential 
energy: 

dV I f I 

= -^TTligOlCl + 77723 I aiCl + -U2C12 

dV I (I 

— - 77 l 2 gO ,2 + 77733 I C*2Cl2 + 2®3Cl23 

dV 1 

^ - T^77l2g02Cl22 

The Euler-Lagrange equations thus reduce to 



+ ms^ I aici + U 2 C 12 + “<^3^123 



+ 11262 + 11262 + 71 - -71 + -777 l30lCl 
+ 77723(oiCl + to2Cl2) + 77l2g(o,lCl + O 2 C 12 + ^ 030123 ) = Tl 
I1261 + I2262 + I2362 + 72 — -72 + -77723O2C12 

+ 77733(02012 + -O3C123) = T2 
I1361 + I2362 + I3363 + 72 — -73 + -77733O3C123 = T3 
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With this example, it becomes apparent that a straightforward differ- 
entiation procedure to derive the Euler-Lagrange equations of a robotic 
manipulator, or for that matter, of a mechanical system at large, is not 
practical. For example, these equations do not seem to lend themselves 
to symbolic manipulations for a six-axis manipulator of arbitrary archi- 
tecture, given that they become quite cumbersome even for a three-axis 
planar manipulator with an architecture that is not so general. For this 
reason, procedures have been devised that lend themselves to an algorith- 
mic treatment. We will study a procedure based on the natural orthogonal 
complement whereby the underlying equations are derived using matrix- 
times- vector multiplications. 



6.3.3 Kane’s Equations 

Kane’s equations (Kane and Levinson, 1983), sometimes referred to as 
D’Alamhert’s equations in Lagrangian form are also useful in robot dynam- 
ics (Angeles et ah, 1989). A feature of Kane’s equations is that they are 
derived from the free-body diagrams of the various rigid bodies constituting 
the multibody system at hand. Upon introducing generalized coordinates 
a la Lagrange, the mathematical model of the system is derived, which is 
equivalent to the underlying Euler-Lagrange equations. Kane’s equations 
take a rather simple form, for an n-dof mechanical system, namely, 

0 + 0 * = 0 



where 4> and (f* are the n-dimensional vectors of generalized active force 
and inertia force, respectively. With the notation introduced above, these 
vectors are given by 




(6.21a) 



(6.21b) 



In the above expressions, q = dq/dt is the n-dimensional vector of gener- 
alized speeds in Kane’s terminology, while the n x 3 matrices dcj/dq and 
duji/dq are the partial rates of change of mass-center velocity and angular 
velocity of the *th rigid body. 



6.4 Recursive Inverse Dynamics 

The inverse dynamics problem associated with serial manipulators is stud- 
ied here. We assume at the outset that the manipulator under study is of 
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the serial type with n + 1 links including the base link and n joints of either 
the revolute or the prismatic type. 

The underlying algorithm consists of two steps: (*) kinematic compu- 
tations, required to determine the twists of all the links and their time 
derivatives in terms of 6 , 6 , and 6 ] and (m) dynamic computations, re- 
quired to determine both the constraint and the external wrenches. Each 
of these steps is described below, the aim here being to calculate the desired 
variables with as few computations as possible, for one purpose of inverse 
dynamics is to permit the real-time model-based control of the manipulator. 
Real-time performance requires, obviously, a low number of computations. 
For the sake of simplicity, we decided against discussing the algorithms 
with the lowest computational cost, mainly because these algorithms, fully 
discussed by Balafoutis and Patel (f99i), rely heavily on tensor calculus, 
which we have not studied here. Henceforth, revolute joints are referred to 
as R, prismatic joints as P. 



6.4-1 Kinematics Computations: Outward Recursions 

We will use the Denavit-Hartenberg (DH) notation introduced in Sec- 
tion 4.2 and hence will refer to Fig. 4.7 for the basic notation required 
for the kinematic analysis to be described first. Note that the calculation 
of each Qj matrix, as given by eq.(4.fe), requires four multiplications and 
zero additions. 

Moreover, every 3-dimensional vector- component transfer from the 
frame to the frame requires a multiplication by Qf . Likewise, ev- 
ery component transfer from the frame to the frame requires a 

multiplication by Qj. Therefore, we will need to account for the afore- 
mentioned component transfers, which we will generically term coordinate 
transformations between successive coordinate frames. We derive below 
the number of operations required for such transformations. If we have 
[r]i = [ri, r' 2 , rp,]"^ £^nd we need [r]j+i, then we proceed as follows: 

[r]j+i = Qf[r]j (6.22) 

and if we recall the form of Qj from eq.(4.fe), we then have 



cos 9i sin 9i 0 




ri 




ri cos 9i + T 2 sin 9i 


— Aj sin 9i Aj cos 9i pi 




r2 


= 


-Xir + pirp 


Pi sin 9i — Pi cos 9i \ 








PiV + Ajra 



(6.23a) 

where \ = cosoj and pi = sinoj, while 

r = r\smOi — rpcosOi (6.23b) 



Likewise, if we have [v]j+i = \v\, V 2 , vp]'^ and we need [v]j, we use the 
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component transformation given below: 





cos 9i 


— Aj sin 9i 


Pi sin 9i 




Vl 




vi cos 9i — V sin 9i 


v]i = 


sin 9i 


Aj cos 9i 


— Pi cos 9i 




V2 


= 


vi sin 9i + V cos 9i 




0 




Ai 








V2Pi + V^Xi 



(6.24a) 

where 

v = V2\i-V3i^i (6.24b) 

It is now apparent that every coordinate transformation between suc- 
cessive frames, whether forward or backward, requires eight multiplications 
and four additions. Here, as in Chapter 4, we indicate the units of multi- 
plications and additions with M and A, respectively. 

The angular velocity and acceleration of the *th link are computed re- 
cursively as follows: 






if the *th joint is R 
(jJi-i, if the *th joint is P 



(6.25a) 






X OiBi + OiBi, if the *th joint is R 

(6.25b) 

if the *th joint is P 



for * = 1, 2, . . . , n, where u>o and loq are the angular velocity and angular 
acceleration of the base link. Note that eqs.(6.25a & b) are frame- invariant; 
i.e., they are valid in any coordinate frame, as long as the same frame is used 
to represent all quantities involved. Below we derive the equivalent relations 
applicable when taking into account that quantities with a subscript i are 
available in J^j+i-coordinates. Hence, operations involving quantities with 
different subscripts require a change of coordinates, which is taken care of 
by the corresponding rotation matrices. 

In order to reduce the numerical complexity of the algorithm developed 
here, all vector and matrix quantities of the *th link will be expressed in 
Note, however, that the two vectors e* and Oj+i are fixed to the *th 
link, which is a potential source of confusion. Now, since e* has very simple 
components in Pi, namely, [0, 0, 1]^, this will be regarded as a vector of 
the (* — l)st link. Therefore, this vector, or multiples of it, will be added to 
vectors bearing the (* — l)st subscript without any coordinate transforma- 
tion. Moreover, subscripted brackets, as introduced in Section 2.2, can be 
avoided if all vector and matrix quantities subscripted with i, except for 
vector 6j, are assumed to be expressed in Jv+i. Furthermore, in view of the 
serial type of the underlying kinematic chain, only additions of quantities 
with two successive subscripts will appear in the relations below. 

Quantities given in two successive frames can be added if both are ex- 
pressed in the same frame, the obvious frame of choice being the frame of 
one of the two quantities. Hence, all we need to add two quantities with 
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successive subscripts is to multiply one of these by a suitable orthogo- 
nal matrix. Additionally, in view of the outwards recursive nature of the 
kinematic relations above, it is apparent that a transfer from IFi- to Jv+i- 
coordinates is needed, which can be accomplished by multiplying either e* 
or any other vector with the (* — 1) subscript by matrix Q^. Hence, the 
angular velocities and accelerations are computed recursively, as indicated 
below: 

+ OiGi)^ if the ith joint is R 

(jJi = i (6.26a) 

1 if the *th joint is P 



I Qf X 

. 

\ QT^i-u 

If the base link is an inertial frame, then 



if the *th joint is R 
if the *th joint is P 



(6.26b) 



Thus, calculating each tUj vector in Jv+i when is given in Pi requires 
8M and 5A if the *th joint is R; if it is P, the said calculation reduces to 8M 
and 4A. Here, note that HjOj = [0, 0, OiY in J^j-coordinates, and hence, 
the vector addition of the upper right-hand side of eq. (6.26a) requires only 
lA. Furthermore, in order to determine the number of operations required 
to calculate u>i in Jv+i when is available in Pi, we note that 



Moreover, we let 



]i 




FIGURE 6.2. A revolute joint. 
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Hence, 



[uji-i X 

Furthermore, we note that 



O-l LOy 

0 



[ ]i 



0 

0 

Oi 



(6.30) 



(6.31) 



and hence, the calculation of thj in when is given in Ti requires 
lOM and 7 A if the *th joint is R; if it is P, the same calculation requires 
8M and AA. 

Furthermore, let Cj be the position vector of Q, the mass center of the 
*th link, being the vector directed from Oi to Q, as shown in Figs. 6.2 
and 6.3. The position vectors of two successive mass centers thus observe 
the relationships 



(*) if the *th joint is R, 



8i^i = aj_i - Pi_-i (6.32a) 

Cj = Cj_i + 5j_i + Pi (6.32b) 

(m) if the *th joint is P, 

5j_i = dj_i - Pi_-i (6.32c) 

Cj = Cj_i + 5j_i + biBi + Pi (6.32d) 

where point Oi, in this case, is a point of the (* — l)st link conveniently 
defined, as dictated by the particular geometry of the manipulator at hand. 
The foregoing freedom in the choice of Oj is a consequence of prismatic pairs 
having only a defined direction but no axis, properly speaking. 

Notice that in the presence of a revolute pair at the *th joint, the differ- 
ence aj_i — Pi_i is constant in Pi. Likewise, in the presence of a prismatic 
pair at the same joint, the difference dj_i — Pi_i is constant in Pi. There- 
fore, these differences are computed off-line, their evaluation not counting 
toward the computational complexity of the algorithm. 

Upon differentiation of both sides of eqs. (6.32b & d) with respect to time, 
we derive the corresponding relations between the velocities and accelera- 
tions of the mass centers of links * — 1 and i, namely, 

(*) if the *th joint is R, 

Cj = Cj_i + Wj_i X 5j_i + Wj X pj (6.33a) 

Cj = Cj_i + thj_i X 5j_i + Wj_i X (Wj_i X 5j_i) +U>iX Pi + 

!jJi X (wj X Pi) (6.33b) 
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FIGURE 6.3. A prismatic joint. 

(n) if the *th joint is P, 

Mi = (6.34a) 

u>i = (6.34b) 

Ui = Si^i + Pi + biBi (6.34c) 

Wi = MiX Uj (6.34d) 

Cj = Cj_i + Vj + biBi (6.34e) 

Cj = Cj_i + u>i X Ui + Mi X (vj + 2biBi) + biBi (6.34f) 

for * = 1, 2, . . . , n, where Cq and Cq are the velocity and acceleration of 

the mass center of the base link. If the latter is an inertial frame, then 

Mo = 0, Mo = 0, Co = 0, Co = 0 (6.35) 



Expressions (6.32b) to (6.34f) are invariant, i.e., they hold in any coor- 
dinate frame, as long as all vectors involved are expressed in that frame. 
However, we have vectors that are naturally expressed in the Pi frame 
added to vectors expressed in the Pi^\ frame, and hence, a coordinate 
transformation is needed. This coordinate transformation is taken into ac- 
count in Algorithm 6.4.1, whereby the logical variable R is true if the *th 
joint is R] otherwise it is false. 

In performing the foregoing calculations, we need the cross product of 
a vector w times e* in Pi coordinates, the latter being simply [ej]j = 
[0, 0, 1]^, and hence, this cross product reduces to [w 2 , —w\, 0]^, whereby 
Wfc, for A: = 1, 2, 3, are the x, y, and z JF^-components of w. This cross prod- 
uct, then, requires no multiplications and no additions. Likewise, vectors 
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biBi, biBi, and 6jej take on the simple forms [0, 0, 6*]^, [0, 0, 6*]^, and 
[0, 0, 6j]^ in J^i. Adding any of these vectors to any other vector in J^i 
then requires one single addition. 

Algorithm 6.4.1 (Outward Recursions); 

read{Qi}"^\ Cq, Wq, Cq, Wq, Cq, {pj?, 

For i = 1 to n step 1 do 
update Qj 
if R then 



Cj ^ 


■ Qf (ci-i + ^i-i) + Pi 




Wj ^ 


QJ + ^i&i) 




Uj-1 V- 


- Wi_i X 




Vj V- 


- Wj X Pi 




Cj ^ 


- Qf (ci_i + Ui_i) + Vi 




thj ^ 


- Qf(thi_i + Wi_i X 0iOi 


+ diBi) 


Cj ^ 

else 


- Qf(cj_i +thi_i X 5j_i 
+U>i X Pi + Wj X Vj 


+ Wi_i X Ui_i) 


Uj ^ 


Qd + Pi + biBi 




Cj ^ 


- Qfci_i + Ui 




Wj ^ 


■ QT^i-1 




Vj V- 


- X Uj 




Wj ^ 


■ biBi 




Cj ^ 


- Qfci_i+Vi+Wi 




thj ^ 


■ 




Cj ^ 

endif 


- Qfci_i + thi X Uj + Wi 


X (Vi + Wi + Wi) + biBi 



enddo 



If, moreover, we take into account that the cross product of two arbitrary 
vectors requires 6M and 3A, we then have the operation counts given below: 

(*) If the *th joint is R, 

Qj requires 4M and OA 
Cj requires 8M and fOA 
u>i requires 8M and 5A 
Cj requires 20M and 16A 
thj requires fOM and lA 
Cj requires 32M and 28A 

(m) If the *th joint is P, 

Qj requires 4M and OA 
Cj requires 16M and 15A 
uji requires 8M and 4A 
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TABLE 6.1. Complexity of the Kinematics Computations 



Item 


M 


A 


{QJi 


An 


0 




8n 


5n 


{cJ? 


20n 


16n 




lOn 


7n 


{cJ? 


32n 


28n 


Total 


82n 


66n 



Cj requires 14M and 11 A 
u>i requires 8M and AA 
Cj requires 20M and IHA 

The computational complexity for the forward recursions of the kinematics 
calculations for an n-revolute manipulator, as pertaining to various algo- 
rithms, are summarized in Table 6.1. Note that if some joints are P, then 
these figures become lower. 



6.4-2 Dynamics Computations: Inward Recursions 

Moreover, a free-body diagram of the end-effector, or nth link, appears 
in Fig. 6.5. Note that this link is acted upon by a nonworking constraint 
wrench, exerted through the nth pair, and a working wrench; the latter 
involves both active and dissipative forces and moments. Although dissipa- 
tive forces and moments are difficult to model because of dry friction and 
striction, they can be readily incorporated into the dynamics model, once 
a suitable constitutive model for these items is available. Since these forces 
and moments depend only on joint variables and joint rates, they can be 
calculated once the kinematic variables are known. For the sake of simplic- 
ity, dissipative wrenches are not included here, their discussion being the 
subject of Section 6.8. Flence, the force and the moment that the (* — l)st 
link exerts on the *th link through the *th joint only produce nonworking 
constraint and active wrenches. That is, for a revolute pair, one has 



~ ry-,X~ 




£X ~ 






Ji 


nf 


f.^ = 

7 H 


fy 


. . 




L ^ J 



(6.36) 



in which nf and rrf are the nonzero JT^-components of the nonworking 
constraint moment exerted by the (* — l)st link on the *th link; obviously, 
this moment lies in a plane perpendicular to Zi, whereas Tj is the active 
torque applied by the motor at the said joint. Vector if contains only 
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FIGURE 6.4. Free-body diagram of the fth link. 



nonworking constraint forces. 
For a prismatic pair, one has 



~ ry-,X ~ 




XX " 






Ji 


nf 


, f^ = 


fy 

J ^ 


_nf _ 




_ _ 



(6.37) 



where vector nf contains only nonworking constraint torques, while Tj is 
now the active force exerted by the *th motor in the Zi direction, /“ and 
being the nonzero JF^-components of the nonworking constraint force 
exerted by the *th joint on the *th link, which is perpendicular to the Zi 
axis. 

In the algorithm below, the driving torques or forces { Tj }", are computed 
via vectors n[ and if . In fact, in the case of a revolute pair, Tj is simply the 
third component of nf ; in the case of a prismatic pair, Tj is, accordingly, 
the third component of if . Prom Fig. 6.5, the Newton-Euler equations of 
the end-effector are 

C = rnn'Cn - f (6.38a) 

= I„u;„ +u;„ X I„w„ - n + p„ x (6.38b) 



where f and n are the external force and moment, the former being applied 
at the mass center of the end-effector. The Newton-Euler equations for the 
remaining links are derived based on the free-body diagram of Fig. 6.4, 
namely, 

ff = nii'Ci + (6.38c) 

nf = Ijthj + Wj X IjWj + + 5j X fj^i + pj X fj^ (6.38d) 

with 5i defined as the difference a* — in eqs.(6.32a & c). 
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n^^link 




FIGURE 6.5. Free-body diagram of the end-effector. 



Once the nf and if vectors are available, the actuator torques and 
forces, denoted by Tj, are readily computed. In fact, if the *th joint is a 
revolute, then 






T P 

ef nf 



(6.39) 



which does not require any further operations, for Tj reduces, in this case, 
to the Zi component of vector nf. Similarly, if the *th joint is prismatic, 
then the corresponding actuator force reduces to 



n 



TpP 



eft: 



(6.40) 



Again, the foregoing relations are written in invariant form. In order to 
perform the computations involved, transformations that transfer coordi- 
nates between two successive frames are required. Here, we have to keep in 
mind that the components of a vector expressed in the (* + l)st frame can 
be transferred to the *th frame by multiplying the vector array in (* + l)st 
coordinates by matrix Qj. In taking these coordinate transformations into 
account, we derive the Newton-Euler algorithm from the above equations, 
namely. 
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Algorithm 6.4.2 (Inward Recursions); 



4 

n^ ^ l„uj„ + uj„x l„uj„ - n + p„ X 

If R then 

'Tn ^ (n^)z 

else 

T„ ^ {i^)z 

For i = n — 1 to 1 step —1 do 

Qifj+i 

if ^ niiCi + 

nf ^ Ijthj + Wj X IjWj + pj X ff + X 

If R then 

n ^ (nf)^ 

else 

Tj ^ fDz enddo 



Note that, within the do-loop of the foregoing algorithm, the vectors to 
the left of the arrow are expressed in the *th frame, while and n|^]^, 
to the right of the arrow, are expressed in the (* + f)st frame. 

In calculating the computational complexity of this algorithm, note that 
the Uj — term is constant in the (* + f)st frame, and hence, it is computed 
off-line. Thus, its computation need not be accounted for. A summary of 
computational costs is given in Table 6.2 for an n-revolute manipulator, 
with the row number indicating the step in Algorithm 6.4.2. 

The total numbers of multiplications Md and additions Ad required by 
the foregoing algorithm are readily obtained, with the result shown below: 

Md = 55n - 22, Ad = 44n - 14 (6.41) 

In particular, for a six-revolute manipulator, one has 

n = 6, Md = 308, Ad = 250 (6.42) 

If the kinematics computations are accounted for, then the Newton-Euler 
algorithm given above for the inverse dynamics of n-revolute manipulators 

TABLE 6.2. Complexity of Dynamics Computations 



Row # 


M 


A 


1 


3 


3 


2 


30 


27 


5 


8(n- 1) 


4(n — 1) 


6 


3(n- 1) 


3(n- 1) 


7 


44(n- 1) 


37(n- 1) 


Total 


55n - 22 


44n — 14 
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requires M multiplications and A additions, as given below: 

M=137n-22, M=110n-14 (6.43) 

The foregoing number of multiplications is identical to that reported by 
Walker and Orin (1982); however, the number of additions is slightly higher 
than Walker and Orin’s figure, namely, lOln — 11. 

Thus, the inverse dynamics of a six-revolute manipulator requires 800 
multiplications and 646 additions. These computations can be performed in 
a few microseconds using a modern processor. Clearly, if the aforementioned 
algorithms are tailored to suit particular architectures, then they can be 
further simplified. Note that, in the presence of a prismatic pair in the jth 
joint, the foregoing complexity is reduced. In fact, if this is the case, the 
Newton-Euler equations for the jth link remain as in eqs. (6.38c & d) for the 
*th link, the only difference appearing in the implementing algorithm, which 
is simplified, in light of the results derived in discussing the kinematics 
calculations. 

The incorporation of gravity in the Newton-Euler algorithm is done most 
economically by following the idea proposed by Luh et al. (1980), namely, 
by declaring that the inertial base undergoes an acceleration — g, where g 
denotes the acceleration of gravity. That is 

Co = -g (6.44) 

the gravitational accelerations thus propagating forward to the EE. A com- 
parison of various algorithms with regard to their computational complex- 
ity is displayed in Table 6.3 for an n-revolute manipulator. For n = 6, the 
corresponding figures appear in Table 6.4. 



6.5 The Natural Orthogonal Complement in Robot 
Dynamics 

In simulation studies, we need to integrate the system of ordinary differ- 
ential equations (ODE) describing the d}mamics of a robotic mechanical 



TABLE 6.3. Complexity of Different Algorithms for Inverse Dynamics 



Author (s) 


Methods 


Multiplications 


Additions 


Hollerbach (1980) 


E-L 


412n- 277 


320n - 201 


Luh et al. (1980) 


N-E 


150n - 48 


131n-48 


Walker & Orin (1982) 


N-E 


137n - 22 


lOln- 11 


Khalil et al. (1986) 


N-E 


105n - 92 


94n — 86 


Angeles et al. (1989) 


Kane 


105n - 109 


90n - 105 


Balafoutis & Patel (1991) 


tensor 


93n — 69 


81n - 65 


Li & Sankar (1992) 


E-L 


88n — 69 


76n — 66 
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TABLE 6.4. Complexity of Different Algorithms for Inverse Dynamics, for n = 6 



Author (s) 


Methods 


Multiplications 
(n = 6) 


Additions 
(n = 6) 


Hollerbach (1980) 


E-L 


2195 


1719 


Luh et al. (1980) 


N-E 


852 


738 


Walker & Orin (1982) 


N-E 


800 


595 


Hollerbach and Sahar (1983) 


N-E 


688 


558 


Kane & Levinson (1983) 


Kane 


646 


394 


Khalil et al. (1986) 


N-E 


538 


478 


Angeles et al. (1989) 


Kane 


521 


435 


Balafoutis & Patel (1991) 


tensor 


489 


420 


Li & Sankar (1992) 


E-L 


459 


390 



system. This system is known as the mathematical model of the system 
at hand. Note that the Newton-Euler equations derived above for a serial 
manipulator do not constitute the mathematical model because we cannot 
use the recursive relations derived therein to set up the underl}dng ODE 
directly. What we need is a model relating the state of the system with its 
external generalized forces of the form 

x = f(x, u), x(to) = xo (6.45) 

where x is the state vector, u is the input or control vector, Xq is the state 
vector at a certain time t^, and f(x, u) is a nonlinear function of x and 
u, derived from the dynamics of the system. The state of a dynamical 
system is defined, in turn, as the set of variables that separate the past 
from the future of the system (Bryson and Ho, 1975). Thus, if we take to 
as the present time, we can predict from eqs.(6.45) the future states of the 
system upon integration of the initial- value problem at hand, even if we 
do not know the complete past history of the system in full detail. Now, if 
we regard the vector 9 of independent joint variables and its time-rate of 
change, 9, as the vectors of generalized coordinates and generalized speeds, 
then an obvious definition of x is 

x=[0^ 0^]'^ (6.46) 

The n generalized coordinates, then, define the configuration of the system, 
while their time-derivatives determine its generalized momentum, an item 
defined in eq.(6.19d). Hence, knowing 9 and 9, we can predict the future 
values of these variables with the aid of eqs.(6.45). 

Below we will derive the mathematical model, eq.(6.45), explicitly, as 
pertaining to serial manipulators, in terms of the kinematic structure of the 
system and its inertial properties, i.e., the mass, mass-center coordinates, 
and inertia matrix of each of its bodies. To this end, we first write the 
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underlying system of uncoupled Newton-Euler equations for each link. We 
have n + 1 links numbered from 0 to n, which are coupled by n kinematic 
pairs. Moreover, the base link 0 need not be an inertial frame; if it is 
noninertial, then the force and moment exerted by the environment upon it 
must be known. For ease of presentation, we will assume in this section that 
the base frame is inertial, the modifications needed to handle a noninertial 
base frame to be introduced in Subsection 6.5.2. 

We now recall the Newton-Euler equations of the *th body in 6-dimen- 
sional form, eqs.(6.5c), which we reproduce below for quick reference: 

Mjtj = -WjMjtj + +wf, * = l,...,n (6.47) 



Furthermore, the definitions of eqs.(6.13b & b) are recalled. Apparently, 
M and W are now 6n x 6n matrices, while t, w^, w'^, and w® are all 
6n-dimensional vectors. Then the foregoing 6n scalar equations for the n 
moving links take on the simple form 

Mt = - WMt + (6.48) 



in which has been decomposed into its active, gravitational, and dissi- 
pative parts w'^, w^, and w®, respectively. Now, since gravity acts at the 
mass center of a body, the gravity wrench 'wf acting on the *th link takes 
the form 



0 

mjg 



(6.49) 



The mathematical model displayed in eq.(6.48) represents the uncoupled 
Newton-Euler equations of the overall manipulator. The following step of 
this derivation consists in representing the coupling between every two 
consecutive links as a linear homogeneous system of algebraic equations on 
the link twists. Moreover, we note that all kinematic pairs allow a relative 
one-degree-of-freedom motion between the coupled bodies. We can then 
express the kinematic constraints of the system in linear homogeneous form 
in the 6n-dimensional vector of manipulator twist, namely. 



Kt = 0 



(6.50) 



with K being a 6n x 6n matrix, to be derived in Subsection 6.5.f. What is 
important to note at the moment is that the kinematic constraint equations, 
or constraint equations, for brevity, eqs.(6.50), consist of a system of 6n 
scalar equations, i.e., six scalar equations for each joint, for the manipulator 
at hand has n joints. Moreover, when the system is in motion, t is different 
from zero, and hence, matrix K is singular. In fact, the dimension of the 
nullspace of K, termed its nullity, is exactly equal to n, the degree of 
freedom of the manipulator. Furthermore, since the nonworking constraint 
wrench produces no work on the manipulator, its sole function being 
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to keep the links together, the power developed by this wrench on t, for 
any possible motion of the manipulator, is zero, i.e., 

= 0 (6.51) 

On the other hand, if the two sides of eq.(6.50) are transposed and then 
multiplied by a 6n-dimensional vector A, one has 

t^K^A = 0 (6.52) 

Upon comparing eqs.(6.51) and (6.52), it is apparent that is of the form 

w^=K^A (6.53) 

More formally, the inner product of and t, as stated by eq.(6.51), 
vanishes, and hence, t lies in the nullspace of K, as stated by eq.(6.50). This 
means that lies in the range of K^, as stated in eq.(6.53). The following 
step will be to represent t as a linear transformation of the independent 
generalized speeds, i.e., as 

t = T6 (6.54) 

with T defined as a 6n x n matrix that can be fairly termed the twist- 
shaping matrix. Moreover, the above mapping will be referred to as the 
twist-shape relations. The derivation of expressions for matrices K and T 
will be described in detail in Subsection 6.5.1 below. Now, upon substitution 
of eq.(6.54) into eq.(6.50), we obtain 

KT6 = 0 (6.55a) 

Furthermore, since the degree of freedom of the manipulator is n, the n 
generalized speeds { Oi }" can be assigned arbitrarily. However, while doing 
this, eq. (6.55a) has to hold. Thus, the only possibility for this to happen is 
that the product KT vanish, i.e., 

KT = O (6.55b) 

where O denotes the 6nxn zero matrix. The above equation states that T is 
an orthogonal complement of K. Because of the particular form of choosing 
this complement — see eq.(6.54) — we refer to T as the natural orthogonal 
complement of K (Angeles and Lee, 1988). 

In the final step of this method, t of eq.(6.48) is obtained from eq.(6.54), 
namely, 

i = Te + T6 (6.56) 

Furthermore, the uncoupled equations, eqs.(6.48), are multiplied from the 
left by T^, thereby eliminating from those equations and reducing 
these to a system of only n independent equations, free of nonworking 
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constraint wrenches. These are nothing but the Euler-Lagrange equations 
of the manipulator, namely, 

le = -T^(MT + WMT)0 + + w^) (6.57) 

where I is the positive definite n x n generalized inertia matrix of the 
manipulator and is defined as 

I = T^MT (6.58) 

which is identical to the inertia matrix derived using the Euler-Lagrange 
equations, with 6 as the vector of generalized coordinates. Now, we let r 
and 5 denote the n-dimensional vectors of active and dissipative generalized 
force. Moreover, we let C(0, 6)6 be the n-dimensional vector of quadratic 
terms of inertia force. These items are defined as 



= 

C(6>, 6) = T^MT + T^'WMT 






t = 5 = T"w^, 7 = T"w 

— I 'tT 



(6.59) 



Clearly, the sum r + 5 produces <p, the generalized force defined in 
eq.(6.11). Thus, the Euler-Lagrange equations of the system take on the 
form 

I0=— C0 + T + 5+7 (6.60) 

If, moreover, a static wrench acts onto the end-effector, with the force 
applied at the operation point, then its effect onto the above model is taken 
into account as indicated in eq.(4.95). Thus, a term is added to the 

right-hand side of the above model: 

10= -C0 + T + 5 + 7 + (6.61) 



As a matter of fact, 6 is defined in eq.(6.59) only for conceptual reasons. 
In practice, this term is most easily calculated once a dissipation function 
in terms of the generalized coordinates and generalized speeds is available, 
as described in Section 6.8. Thus, 6 is computed as 




It is pointed out that the first term of the right-hand side of eq.(6.60) 
is quadratic in 6 because matrix C, defined in eq.(6.59), is linear in 6. In 
fact, the first term of that expression is linear in a factor T that is, in turn, 
linear in 6. Moreover, the second term of the same expression is linear in 
W, which is linear in 6 as well. However, C is nonlinear in 6. Because of 
the quadratic nature of that term, it is popularly known as the vector of 
Coriolis and centrifugal forces, whereas the left-hand side of that equation 
is given the name of vector of inertia forces. Properly speaking, both the 
left-hand side and the first term of the right-hand side of eq.(6.60) arise 
from inertia forces. 
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Example 6.5.1 (A minimum-time trajectory) A pick-and-place oper- 
ation is to he performed with an n-axis manipulator in the shortest possible 
time. Moreover, the maneuver is defined so that the n-dimensional vector 
of joint variables is given by a common shape function s{x), with 0 < x < 1 
and 0 < s < 1, which is prescribed. Thus, for a fixed n-dimensional vector 
6q, the time-history of the joint-variable vector, 9{f), is given by 

e{t) = eo + s(^']Ae, o<t<T 



with T defined as the time taken by the maneuver, while 6 q and 6 q + A6 
are the values of the joint-variable vector at the pick- and the place-postures 
of the manipulator, respectively. These vectors are computed from inverse 
kinematics, as explained in Chapter f. Furthermore, the load-carrying ca- 
pacity of the manipulator is specified in terms of the maximum torques 
delivered by the motors, namely, 

\n\ <~i, for * = 1, . . . , n 



where the constant values Ti are supplied by the manufacturer. In order to 
keep the analysis simple, we neglect power loses in this example. Find the 
minimum time in which the maneuver can take place. 

Solution: Let us first calculate the vector of joint-rates and its time-deriv- 
ative: 

e{t) = ^s\x)Ae, e{t) = ^s'\x)Ae, x = ^ 

Now we substitute the above values into the mathematical model of eq.(6.60), 
with S{t) = 0, thereby obtaining 

T = 1(6)6 + C(e, e)e^.s"(x)i(x)Ae + ^.s'\x)c(x)Ae ^ ^i(x) 
with i(x) defined, of course, as 

i(x) = [I(x)s"(x) + C(x)s'^(x)]A0 

the 1/T^ factor in the term of Coriolis and centrifugal forces stemming 
from the quadratic nature of the C(9, 9)9 term. What we now have is the 
vector of motor torques, r, expressed as a function of the scalar argument 
X. Now, let fi(x) be the *th component of vector f(x), and 

Ej = max{|/j(x)|}, for *=l,...,n 

X 



We would then like to have each value Fi produce the maximum available 
torque Tj, namely. 



Ti 



Fi 



y2 ’ 



* = 1, . . . n 
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and hence, for each joint we have a value Tj of T given by 



—— 



Ei 



* = 1, . . . n 



Obviously, the minimum value sought, Tmin, is nothing but the maximum 
of the foregoing values, i.e.. 



Train = max{Tj}" 
i 



thereby completing the solution. 

6.5.1 Derivation of Constraint Equations and 
Twist-Shape Relations 

In order to illustrate the general ideas behind the method of the natural or- 
thogonal complement, we derive below the underlying kinematic constraint 
equations and the twist-shape relations. We first note, from eq. (6.25a), that 
the relative angular velocity of the *th link with respect to the (* — l)st 
link, uji — (jJi-i, is HjOj. Thus, if matrix Ej is defined as the cross-product 
matrix of vector e*, then, the angular velocities of two successive links obey 
a simple relation, namely. 



Ej(wj - = 0 (6.63) 

Furthermore, we rewrite now eq.(6.33a) in the form 

Cj - Cj_i + RjWj + = 0 (6.64) 

where Dj and Rj are defined as the cross-product matrices of vectors Si, 
defined in Subsection 6.4.1 as a* — Pi, and Pi, respectively. In particular, 
when the first link is inertial, eqs.(6.63 & b), as pertaining to the first link, 
reduce to 



Eitui = 0 (6.65a) 

Cl + Ri^i = 0 (6.65b) 

Now, eqs.(6.63) and (6.64), as well as their counterparts for * = 1, 
eqs.(6.65a & b), are further expressed in terms of the link twists, thereby 
producing the constraints below: 



Kiiti — 0 

T Klltl 0, ^ \ , . . . , Tl 

with Kii and Kjj, for * = 2, . . . , n and j = i — 1, i, defined as 



Kii 



El O 
Ri 1 



(6.66a) 

(6.66b) 



(6.67a) 
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-Ei O 

Di 1 -1 



Ku 



Ei O 

Ri 1 



(6.67b) 

(6.67c) 



where 1 and O denote the 3x3 identity and zero matrices, respectively. 
Furthermore, from eqs. (6.66a & b) and (6.67a-c), it is apparent that matrix 
K appearing in eq. (6.55b) takes on the form 



K 



fKii 


Oe 


Oe 


Oe 


Oe 1 


K21 


K22 


Oe 


Oe 


Oe 


Oe 


Oe 


Oe 


' ' ' 


•• 0 


L Oe 


Oe 


Oe 


I^n,n— 1 


~^nn - 



( 6 . 68 ) 



with Oe denoting the 6x6 zero matrix. 

Further, the link-twists are expressed as linear combinations of the joint- 
rate vector 9 . To this end, we define the 6 x n partial Jacobian Jj as the 
matrix mapping the joint-rate vector 6 into the twist tj of that link, i.e.. 



J,6> 



(6.69) 



whose jth column, tjj, is given, for i, j 




1, 2, . . . , n, by 

if j < *; 

otherwise. 



with Yij illustrated in Fig. 6.6 and defined, for i, j 



+ ^j+i + • • • + + p^, 

Pi 1 



= 1, . . . , n, as 
if j < i] 
if j = i] 



otherwise. 



(6.70) 



(6.71) 



We can thus readily express the twist tj of the *th link as a linear com- 
bination of the first i joint rates, namely. 



tj — eitji + C2ti2 

and hence, matrix T of eq.(6.54) takes the form 

til 0 ••• 0 

I 1 

T = 



i^ii 7 i 1 , . . . , 7?. 



^21 1'22 



0 



^n2 



• • • t^ 



(6.72) 



(6.73) 



As a matter of verification, one can readily prove that the product of ma- 
trix T, as given by eq.(6.73), by matrix K, as given by eq.(6.68), vanishes, 
and hence, relation (6.55b) holds. 
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FIGURE 6.6. Kinematic subchain comprising links j, j + 1 . . . , i. 



The kinematic constraint equations on the twists, for the case in which 
the *th joint is prismatic, are derived likewise. In this case, we use eqs. (6.34a 
& e), with the latter rewritten more conveniently for our purposes, namely, 

uji = (6.74a) 

Cj = Cj_i + X (5j_i + Pi + biBi) + biBi (6.74b) 

We now introduce one further definition: 



R'=D'_i+Ri (6.75) 

where is the cross-product matrix of vector defined in Subsec- 

tion 6.4.1 as dj_i — Pi_i, while Rj is the cross-product matrix of p^ + biBi. 
Hence, eq.(6.74b) can be rewritten as 

Cj - Cj_i + R'wj - biBi = 0 (6.76) 



Upon multiplication of both sides of eq.(6.76) by Ej, the term in 6j cancels, 
and we obtain 

Ei(cj - Cj_i + R'wj) = 0 (6.77) 

Hence, eqs. (6.74a) and (6.77) can now be regrouped in a single 6-dimen- 
sional linear homogeneous equation in the twists, namely. 



the associated matrices being defined below: 



-1 


O 


o 


-Ej 


1 


O 


EjR) 


Ej 



(6.78) 



(6.79a) 

(6.79b) 
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with 1 and O defined already as the 3x3 identity and zero matrices, 
respectively. If the first joint is prismatic, then the corresponding constraint 
equation takes on the form 

K'liti = 0 (6.80) 



with defined as 



K 



11 



1 O 
O El 



(6.81) 



Furthermore, if the A:th pair is prismatic and 1 < k < i, then the twist 
tj of the ith link changes to 



tj — f^itji + • • • + + • • • + * — 1, . . . , n (6.82) 



where t'j, is defined as 




(6.83) 



In order to set up eq.(6.60), then all we now need is T, which is computed 
below. Two cases will be distinguished again, namely, whether the joint at 
hand is a revolute or a prismatic pair. In the first case, from eq.(6.70) one 
readily derives, for *, j = 1, 2, . . . , n. 



X Qj 

X 6j) X Tij + X Tij 

o' 

0 ’ 



, if i < *; 

(6.84) 

otherwise 



where, from eq.(6.71), 

Yij =ujj X a.j -\ h X aj_i +MiX Pi (6.85) 



On the other hand, if the A:th pair is prismatic and I < k < i, then from 
eq.(6.83), the time-rate of change of t'^, becomes 



0 









( 6 . 86 ) 



thereby completing the desired derivations. 

Note that the natural orthogonal complement can also be used for the 
inverse dynamics calculations. In this case, if the manipulator is subjected 
to a gravity field, then the twist-rate of the first link will have to be modified 
by adding a nonhomogeneous term to it, thereby accounting for the gravity- 
acceleration terms. This issue is discussed in Section 6.7. 
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6.5.2 Noninertial Base Link 

Noninertial bases occur in space applications, e.g., in the case of a manip- 
ulator mounted on a space platform or on the space shuttle. A noninertial 
base can be readily handled with the use of the natural orthogonal comple- 
ment, as discussed in this subsection. Since the base is free of attachments 
to an inertial frame, we have to add its six degrees of freedom (dof) to the 
n dof of the rest of the manipulator. Correspondingly, t, w^, w'^, and w® 
now become 6(n+ l)-dimensional vectors. In particular, t takes the form 

t = [tj tf ... t^r (6.87) 

with to defined as the twist of the base. Furthermore, the vector of in- 
dependent generalized speeds, 0, is now of dimension n + 6, its first six 
components being those of to, the other n remaining as in the previous 
case. Thus, 6 has the components shown below: 

0=[tl ... (6.88) 

Correspondingly, T becomes a 6(n + 1) x (n + 6) matrix, namely, 

where 1 is the 6x6 identity matrix, O denotes the 6 x n zero matrix. O' 
represents the 6n x 6 zero matrix, and T' is the 6n x n matrix defined in 
eq.(6.73) as T. Otherwise, the model remains as in the case of an inertial 
base. 

A word of caution is in order here. Because of the presence of the twist 
vector to in the definition of the vector of generalized speeds above, the 
latter cannot, properly speaking, be regarded as a time-derivative. Indeed, 
as studied in Chapter 3, the angular velocity appearing in the twist vector is 
not a time-derivative. Hence, the vector of independent generalized speeds 
defined in eq.(6.88) is represented instead by v, which does not imply a 
time-derivative, namely, 

v=[t(^ i)i ••• Onf' (6.90) 

6.6 Manipulator Forward Dynamics 

Forward dynamics is needed either for purposes of simulation or for the 
model-based control of manipulators (Craig, 1989), and hence, a fast cal- 
culation of the joint- variable time- histories 6{t) is needed. These time- 
histories are calculated from the model displayed in eq.(6.61), reproduced 
below for quick reference, in terms of vector 0(t), i.e., 

le = -C(6>, 6)6 + r(t) + 5{e, 0) + j{0) + 



(6.91) 
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Clearly, what is at stake here is the calculation of 9 from the foregoing 
model. Indeed, the right-hand side of eq.(6.91) can be calculated with the 
aid of the Newton-Euler recursive algorithm, as we will describe below, 
and needs no further discussion for the time being. Now, the calculation 
of 9 from eq.(6.91) is similar to the calculation of 9 from the relation 
between the joint-rates and the twist, derived in Section 4.5. From the 
discussion in that section, such calculations take a number of floating-point 
operations, or flops, that is proportional to n®, and is thus said to have a 
complexity of O(n ^) — read “order n®.” In real-time calculations, we would 
like to have a computational scheme of 0(n). In attempting to derive such 
schemes. Walker and Orin (1982) proposed a procedure that they called the 
composite rigid-body method, whereby the number of flops is minimized by 
cleverly calculating 1 ( 0 ) and the right-hand side of eq.(6.91) by means of the 
recursive Newton-Euler algorithm. In their effort, they produced an 0{gn?) 
algorithm to calculate 9. Thereafter, Featherstone (1983) proposed an 0(n) 
algorithm that is based, however, on the assumption that Coriolis and 
centrifugal forces are negligible. The same author reported an improvement 
to the aforementioned algorithm, namely, the articulated-body method, that 
takes into account Coriolis and centrifugal forces (Featherstone, 1987.) The 
outcome, for an n-revolute manipulator, is an algorithm requiring 300n — 
267 multiplications and 279n — 259 additions. For n = 6, these figures yield 
1,533 multiplications and 1,415 additions. Li (1989) reported an O(n^) 
algorithm leading to 783 multiplications and 670 additions. 

In this subsection, we illustrate the application of the method of the 
natural orthogonal complement to the modeling of an n-axis serial ma- 
nipulator for purposes of simulation. While this algorithm gives an O(n^) 
complexity, its derivation is straightforward and gives, for a six-axis ma- 
nipulator, a computational cost similar to that of Featherstone’s, namely, 
1,596 multiplications and 1,263 additions. Moreover, a clever definition of 
coordinate frames leads to even lower figures, i.e., 1,353 multiplications and 
1,165 additions, as reported by Angeles and Ma (1988). Further develop- 
ments on robot dynamics using the natural orthogonal complement have 
been reported by Saha (1997, 1999), who proposed the decoupled natural 
orthogonal complement as a means to enable the real-time inversion of the 
mass matrix. 

The manipulator at hand is assumed to be constituted by n moving links 
coupled by n kinematic pairs of the revolute or prismatic types. Again, 
for brevity, the base link is assumed to be inertial, noninertial bases be- 
ing readily incorporated as described in Subsection 6.5.2. For the sake of 
conciseness, we will henceforth consider only manipulators mounted on an 
inertial base. Moreover, we assume that the generalized coordinates 9 and 
the generalized speeds 9 are known at an instant tk, along with the driving 
torque r(t), for t > ti~, and of course, the DH and the inertial parameters 
of the manipulator are assumed to be known as well. Based on the forego- 
ing information, then, 9 is evaluated at tk and, with a suitable integration 
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scheme, the values of 9 and 9 are determined at instant tk+i- Obviously, 
the governing equation (6.60) enables us to solve for 9{tk)- This requires, 
of course, the inversion of the n x n matrix of generalized inertia I. Since 
the said matrix is positive-definite, solving for 9 from eq.(6.60) can be done 
economically using the Cholesky-decornposition algorithm (Dahlquist and 
Bjorck, 1974). The sole remaining task is, then, the computation of I, the 
quadratic inertia term C0, and the dissipative torque 5. The last of these 
is dependent on the manipulator and the constitutive model adopted for 
the representation of viscous and Coulomb friction forces and will not be 
considered at this stage. Models for dissipative forces will be studied in 
Section 6.8. Thus, the discussion below will focus on the computation of I 
and C9 appearing in the mathematical model of eq.(6.91). 

Next, the 6n x 6n matrix M is factored as 

M = H^H (6.92) 

which is possible because M is at least positive-semidefinite. In particular, 
for manipulators of the type at hand, M is positive-definite if no link-mass 
is neglected. Moreover, due to the diagonal-block structure of this matrix, 
its factoring is straightforward. In fact, H is given simply by 

H = diag(Hi, . . . ,H„) (6.93) 

each 6x6 block Hj of eq.(6.93) being given, in turn, as 

N O 

Hi= , (6.94) 

O n j 1 

with 1 and O defined as the 3x3 identity and zero matrices, respectively. 
We thus have 

Mi = HfHi (6.95) 

Furthermore, Nj can be obtained from the Cholesky decomposition of Ij, 
while Ui is the positive square root of m*, i.e., 

Ij=NfNj, nii = nj (6.96) 

Now, since each 6x6 Mj block is constant in body-fixed coordinates, the 
above factoring can be done off-line. Prom the foregoing definitions, then, 
the n X n matrix of generalized inertia I can now be expressed as 

I = P^P (6.97) 

where P is defined, in turn, as the 6n x n matrix given below: 



P = HT 



(6.98) 
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The computation of P is now discussed. If we recall the structure of T 
from eq.(6.73) and that of H from eq.(6.93), along with the definition of 
P, eq.(6.98), we readily obtain 



P 



■ Hitii 


0 


0 - 




■pll 


0 


0 - 


H2t21 


H2t22 


0 


= 


P21 


P22 • 


0 


- 


^n^n2 






-Pnl 


Pn2 • 


Pnn - 



with 0 denoting the 6-dimensional zero vector. Moreover, each of the above 
nontrivial 6-dimensional arrays pij is given as 



NjOj 



Pb — 



if the jth joint is R] 
if the jth joint is P 



( 6 . 100 ) 



Thus, the (i,j) entry of I is computed as the sum of the inner products 
of the {k, i) and the ik^j) blocks of P, for A: = j, . . . , n, i.e.. 



h = Iji = 13 PliPkj ( 6 . 101 ) 

k=j 

with both pfcj and py expressed in JFj,^j^_coordinates, i.e., in A:th-link co- 
ordinates. Now, the Cholesky decomposition of I can be expressed as 

I = L^L (6.102) 



where L is an n x n lower-tri angular matrix with positive diagonal entries. 
Moreover, eq.(6.91) is now rewritten as 

L^L0= -(C0- -7) + 5 + t (6.103) 

If we now recall eq.(6.91), it is apparent that the term inside the parentheses 
in the right-hand side of the above equation is nothing but the torque 
required to produce the motion prescribed by the current values of 6 and 
0, in the absence of dissipative wrenches and with zero joint accelerations, 
when the manipulator is acted upon by a static wrench w^. That is, if we 
call T the torque r of eq.(6.91) under the foregoing conditions, then 

C0-J^w^-7 = r| A =T (6.104) 

' 'v,^=o,6=o ^ ’ 

which is most efficiently computed from inverse dynamics, using the recur- 
sive Newton-Euler algorithm, as described in Section 6.4 . Now eq.(6.102) 
is solved for 6 in two steps, namely. 



L^x = — T + T + 5 

L0 = X 



(6.105a) 

(6.105b) 
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In the above equations, then, x is first computed from eq.(6.105a) by 
backward substitution. With x known, 9 is computed from eq.(6.105b) 
by forward substitution, thereby completing the computation of 9 . The 
complexity of the foregoing algorithm is discussed in Subsection 6.6.2. 

Alternatively, 9 can be calculated in two steps from two linear systems 
of equations, the first one underdetermined, the second over determined. 
Indeed, if we let the product P0 be denoted by y, then the dynamics 
model of the manipulator, eq.(6.60), along with the factoring of eq.(6.97), 
leads to 



P^y = — T + T + 5 (6.106a) 

P0 = y (6.106b) 

Thus, in the above equations, y is calculated first as the rninirnurn-norrn 
solution of eq.(6.106a); then, the desired value of 9 is calculated as the 
least-square approximation of eq.(6.106b). These two solutions are com- 
puted most efficiently using an orthogonalization algorithm that reduces 
matrix P to upper-triangular form (Golub and Van Loan, 1989). A straight- 
forward calculation based on the explicit calculation of the generalized 
inverses involved is not recommended, because of the frequent numerical 
ill-conditioning incurred. Two orthogonalization procedures, one based on 
Householder reflections, the other on the Gram-Schmidt procedure, for the 
computation of both the least-square approximation of an overdetermined 
system of equations and the minimum- norm solution of its underdetermined 
counterpart are outlined in Appendix B. 

The complexity of the foregoing calculations is discussed in Subsec- 
tion 6.6.2, based on the Cholesky decomposition of the generalized iner- 
tia matrix, details on the alternative approach being available elsewhere 
(Angeles and Ma, 1988). 



6.6.1 Planar Manipulators 



The application of the natural orthogonal complement to planar manipu- 
lators is straightforward. Here, we assume that the manipulator at hand is 
composed of n links coupled by n joints of the revolute or the prismatic 
type. Moreover, for conciseness, we assume that the first link, labeled the 
base, is fixed to an inertial frame. We now adopt the planar representation 
of the twists and wrenches introduced in Section 4.8; that is, we define the 
twist of the *th link and the wrench acting on it as 3-dimensional arrays, 
namely. 




(6.107) 



where uji is the scalar angular velocity of this link; Cj is the 2-dimensional 
velocity of its mass center, Q; n-j is the scalar moment acting on the link; 
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and fj is the 2-dimensional force acting at Q. Moreover, the inertia dyad 
is now a 3 X 3 matrix, i.e.. 



Mi 



li 



0^1 



0 mjl 



(6.108) 



with li defined as the scalar moment of inertia of the *th link about an axis 
passing through its center of mass, in the direction normal to the plane of 
motion, while 0 is the 2-dimensional zero vector and 1 is the 2x2 identity 
matrix. 

Furthermore, the Newton-Euler equations of the *th link take on the 
forms 



Ui = liLUi (6.109a) 

fj = mjCj (6.109b) 

and so, these equations can now be cast in the form 

Mjtj = -h wf , * = l,...,n (6.110) 

where we have decomposed the total wrench acting on the *th link into its 
working component w)^, supplied by the environment and accounting for 
motor and joint dissipative torques, and w)^, the nonworking constraint 
wrench, supplied by the neighboring links via the coupling joints. The lat- 
ter, it is recalled, develop no power, their sole role being to keep the links 
together. An essential difference from the general 6-dimensional counter- 
part of the foregoing equation, namely, eq.(6.48), is the lack of a quadratic 
term in u)i in eq.(6.109a) and consequently, the lack of a WjMitj term in 
eq.(6.110). 

Upon assembling the foregoing 3n equations of motion, we obtain a sys- 
tem of 3n uncoupled equations in the form 

Mt = 

Now, the wrench accounts for active forces and moments exerted on 
the manipulator, and so we can decompose this wrench into an actuator- 
supplied wrench and a gravity wrench w^. 

In the next step of the formulation, we set up the kinematic constraints 
in linear homogeneous form, as in eq.(6.50), with the difference that now, 
in the presence of n kinematic pairs of the revolute or the prismatic type, 
K is a 3n X 3n matrix. Moreover, we set up the twist-shape relations in the 
form of eq.(6.56), except that now, T is a 3n x n matrix. The derivation of 
the Euler-Lagrange equations for planar motion using the natural orthog- 
onal complement, then, parallels that of general 3-dimensional motion, the 
model sought taking the form 

i{e)e + c{6 ,e)e = T + ■^ + 5 



(6.111a) 
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with the definitions 

I( 6 >) = T^MT, C( 6 >, 0) = T^MT, (6.111b) 

r = T^w"^, 7 = T^w^, S = T'^w^ (6.111c) 

We can illustrate best this formulation with the aid of the example below. 

Example 6.6.1 (Dynamics of a planar three-revolute manipulator) 

Derive the model of the manipulator of Fig. f.2f, under the assumptions 
of Example 6.3.1, hut now using the natural orthogonal complement. 

Solution: We start by deriving all kinematics-related variables, and thus, 

1^1 = f^i, lv 2 = -\- 62 , Lv^, = -\- 62 + O 3 

Furthermore, 

ti = Oitii 

t2 = 0it\2 + f^2t22 

ta = S\ti3 + <?2t23 + f^3t33 

where 



til = 


1 

_Erii_ 


= 


1 

Epi 


= 


1 

_(l/2)Eai_ 




t 21 = 


1 

_Eri 2 _ 


= 


1 

_E(ai + P 2 ) _ 


= 


1 

_E(ai + (l/ 2 )a 2 )_ 






1 




1 






1 


1 




t 22 = 


_ Er 22 _ 




_Ep 2 _ 




_(l/ 2 )Ea 2 _ 




tai = 


1 

_Eri 3 _ 


= 


1 

E(ai + a 2 + P 3 ) 


= 


1 

E(ai + a 2 + (l/ 2 )a 3 ) 




1 




r 


1 


1 






1 1 




t32 = 


_ Er23 _ 




|_E(a 2 + P 3 ) _ 




_E(a 2 + (l/ 2 )a 3 ) _ 






1 




1 












t33 = 


_Ep3. 




(l/ 2 )Ea 3 _ 











and hence, the 9x3 twist-shaping matrix T becomes 



T 



1 0 

(l/2)Eai 0 

1 1 

E(ai + (l/2)a2) (l/2)Ea2 

1 1 



0 

0 

0 

0 

1 



L E(ai + a 2 + (l/2)a3) E(a 2 + (l/2)a3) (l/2)Ea3J 

The 9x9 matrix of inertia dyads of this manipulator now takes the form 



M = diag(Mi, M 2 , M 3 ) 
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with each 3x3 Mj matrix defined as 

M, s [f; 

Now, the 3x3 generalized inertia matrix is readily derived as 

I = T^MT 

whose entries are given below: 

/ii = 

Il2 = t^iM2t22 + t^]^M3t32 = I 2 I 

hs = = hi 

I22 = t^2M2t22 + 1^2^3132 
hs = ts2 Matas = I32 

I 33 = ts3M3t33 



0^ ■ 
rriil 



Upon expansion, the above entries result in exactly the same expressions 
as those derived in Example 6.3.1, thereby confirming the correctness of 
the two derivations. Furthermore, the next term in the Euler-Lagrange 
equations is derived below. Here, we will need T, which is readily derived 
from the above expression for T. In deriving this time-derivative, we note 
that in general, for * = 1,2, 3, 






and hence, 

0 

(l/2)0iai 

T = - • • 

<?iai + (l/2)0i2a2 

0 

.^lai -h <?i2a2 + (l/2)0i23a3 



0 0 

0 0 

0 0 

(l/2)fii2a2 0 

0 0 



<?12a2 + (l/2)0i23a3 (l/2)0i23a3 J 



where 0i2 and 6123 stand for 9i + 62 and 0\ + 62 + 0^,, respectively. 

We now can perform the product T^MT, whose (*,j) entry will be 
represented as p,jj. Below we display the expressions for these entries: 

Mil = --jhi2a,ia,2S2 + m 3 ( 2 aia 2 S 2 + aiaaS 2 a)]f ^2 ^(aiaaS 2 a + 0,20,383)63 

M 12 = -^[TO 2 aia 2 S 2 + m3(2aia2S2 + aiaaS 2 a)](^i + h) 

— -m3(o,lO,3S23 + 0 , 20 , 383)63 
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Mi3 = -^m3(aia3S23 + a2a3S3)(^i + ^2 + O3) 

M 21 = 2 ["'' 2 aia 2 S 2 + m3(2aia2S2 + aia3S23)]^^i ~ 2’^3'2'2a3S3^^3 

M 22 = — 2*2^3 02 03 S 3 ^^3 

M 23 = -^TO3a2a3S3(^i + O 2 + O 3 ) 

M31 = 2"''3[(a'i'2'3S23 + a2a3S3)^^i + a2a3S3^^2] 

M32 = ^TO3a2a3S3(^i + ^2) 

M33 = 0 

Now, let us define 

1/ = T^MT0 

whose three components are given below: 



vi = -[m2a,ia2S2 + m 3 ( 2 aia 2 S 2 + aia 3 S 23 )]^i ^2 - ^3(0103523 + 0,20,383)6163 
-i[m2aia2S2 + m3(2aia2S2 + aia3S23)]^2 

— 7713(0103523 + 0,20,383)6263 — -7773(0103523 + 0,20,383)63 

= 2 N2O1O252 + 7773(2010252 + OiO3523)]0i - 7773020353^103 

• • 1 •„ 

- 17130 , 20,3836263 - - 7773 O 2 O 35303 

V3 = ^7773(0103523 + 0,20,383)61 + 17130203836162 + ^171302038361 

The mathematical model sought, thus, takes the form 



I(6>)6> + i/(6>,6>) =T+7 



where 5 = 0 because we have not included dissipation. Moreover, 7 is 
derived as described below: Let be the gravity wrench acting on the 
ith link, then being 



w 



G 



W 

w 

w 



G 1 

1 

G 

2 

G 

3 



and 



0 


G 


0 


G 


0 


_-i 7 iigj_ 


, W 2 = 


_-« 725 'j_ 


. W 3 = 


-« 735 'j_ 



Therefore, 
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7 = 

r miaf Ej + m2(2ai + a2)^Ej + m3[2(ai + a2) + a3)^Ej 
= - TO2af Ej + m3(2a2 + a3)^Ej 

^ L rnsa^Ej 



But 



Hence, 



9 






2 



a^f Ej = — a^f i = — ai cos 0i 

8L2 Ej = i = -02 COs( 6 »i + 02) 

ag Ej = -ag i = -og cos( 6 »i + 62 + 03) 



—niiaici — 2m2(aiCi + OgCig) — 2 mg(aiCi + OgCig + O3C123) 
— mgOgCig — 2 mg(a 2 Ci 2 + 030123) 

— mg03Ci23 



with the definitions for ci, ci 2 , and C 123 introduced in Example 6.3.1. As 
the reader can verify, the foregoing model is identical to the model derived 
with the Euler-Lagrange equations in that example. 



Example 6.6.2 r] (Dynamics of a spatial 3-revolute manipulator) 

The manipulator of Fig. f.l5 is reproduced in Fig. 6.7, in a form that 
is kinematically equivalent to the .sketch of that figure, hut more .suitable 
for the purposes of this example. For this manipulator, (i) find its inertia 
matrix at the configuration depicted in that figure; (ii) find the time-rate 
of change of the inertia matrix under a maneuver whereby 0 \ = 62 = 63 = 
p s^^ and 01 = 02 = (^3 = 0 ; and (in) under the same maneuver, find 
the centrifugal and Coriolis terms of its governing equation. Furthermore, 
assume that all links are identical and dynamically isotropic. What we mean 
by “dynamically isotropic” is that the moment of inertia of all three links 
about their mass centers are proportional to the 3x3 identity matrix, the 
proportionality factor being F Moreover, all three links are designed so that 
the mass center of each is located as shown in Fig. 6 . 7. 



Solution: 

(i) Henceforth, we represent all vectors and matrices with respect to the 
JFj^-frame of Fig. 6.7, while denoting by i, j, and k the unit vectors 
parallel to the Xi, Yi, and Zi axes, respectively. Under these condi- 
tions, we have, for the unit vectors parallel to the revolute axes, 

ei=k, 02 =j, eg = i 

while vector a* is directed from the origin of JF* to that of Yj+i, for 
* = 1,2, 3. Hence, 

ai = — ai, ag = a(j — k), ag = a(i + k) 
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FIGURE 6.7. Mass-center locations of the manipulator of Fig. 4.19. 



Likewise, the position vectors of the mass centers, for * = 1, 2 , 
and 3, with respect to the origins of their respective frames, are given 

by 

Pi = 

P2 = ^«(i + 2 j - k) 

P3 = ^«(2i + k) 

We can now calculate the various 6-dimensional arrays tjj, for * = 1, 
2 , 3, and j = \ till *, i.e.. 



til 

t21 

t22 

tai 

t32 

t33 



61 k 

ei X pj ^ [-(a/2)(i+j)_ 

61 k 

ei X (ai + P2)J ^ |_-(a/ 2 )( 2 i + j)_ 

62 1 ^ r j 

62xp2j |_-(a/2)(i + k) 

ei k 

ei X (ai + a2 + pg) J 

62 1 ^ r j 

62x(a2 + Pg)J |_-(a/2)(i + 2k) 

63 1 ^ r i 

63XP3J |_-(a/2)j_ 
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and so, the 18 x 3 matrix T is given by 



k 0 0 

-(a/2)(i+j) 0 0 

k j 0 

-(a/2)(2i + j) -(a/2)(i + k) 0 

k j i 

-ai -(a/2)(i + 2k) -(a/2)j 



Moreover, the 6 x 6 inertia dyad of the *th link takes the form 

Mi= ^ ® , * = 1,2,3 

O ml 

with 1 and O denoting the 3x3 identity and zero matrices, respec- 
tively. Thus, the 18 x 18 system mass matrix is given as 

M = diag(Mi, M 2 , M 3 ) 

and the 3x3 generalized inertia matrix I of the manipulator is 

I = T^MT 

whose entries are given by 

111 = 

1 12 = t^iM 2 t 22 + t)^]^M3t32 = I 21 

1 13 = = I 31 

122 = t^2^2t22 +132^3132 

1 23 = t32 Matas = I 32 
I 33 = ts 3 M 3 t 33 

Upon expansion, the foregoing expressions yield 

, [11 40] [30 o' 

I = -me? 4 70 +/020 

^ [o 0 ij [001 

(**) Now, the time-rate of change of I, I, is calculated as 

i = T^MT + t^MT^ + T^(WM - MW)T 

We proceed first to compute T. This time-derivative is nothing but 
the 18x3 matrix whose entries are the time-derivatives of the entries 
of T, namely, tjj, as given in eq.(6.84), which is reproduced below for 
quick reference: 

{..= [ 

[i^j X ej) X Tij + ej X Tij _ 
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where tij is given, in turn, by 

Yij = ujj X a.j + . . . + X aj_i +MiX Pi 

Hence, we will need vectors tUj, for * = 1, 2, and 3. These are calcu- 
lated below: 



u>\ = 9ie\ = pk 

u >2 = OiQi + 0202 = p{} + k) 

W3 = 0161 + 0262 + 0363 = p(i + j + k) 



We have, therefore, 



til 

t21 



6 l 

6i X Pi + 6i X Pi 



6i X (Wi X Pi) 



(f/2)a(i-j) 



6 l 



L-22 — 



6i X (ai + P2) + 61 X (ai + P2 

0 

61 X (tUi X ai + tU2 X P2 

62 

62 X P2 + 62 X P2 



= P 



0 

(l/2)aj 



= P 



tai 



= P 



pei X 62 

(p6i X 62) X P2 + 62 X [p(6i + 62) X P2] 

— i 

-(l/2)a(i + j -k) 

6l 

6i X (ai + a2 + P3) + 61 X (ai + A2 + 

0 

61 X {u>i X ai + W2 X a2 + W3 X p^) 

0 

61 X [p6i X ai +p(6i + 62) X a2 +p(6i + 62 + 63) X Pg] 

0 



-aj 



( 6 . 112 ) 



'.'32 



62 

62 X (a2 + P3) + 62 X (a2 + P3) 

P6i X 62 

(p6i X 62) X (a2 +P3) +P62 X [(61 +62) X (a2 + P3)] 



—1 



-(l/2)a(2i+j-k) 
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t 33 



eg 




LO2 X eg 


eg X Pg -h eg X Pg 




(W2 X eg) X Pg -h eg X (wg X Pg) 



p{ei +62) X 63 

p[{ei +62) X 63] X P3 +pe3 X [(ei +62 +63) x P3] 
p{b 2 - ei) 

p{e2 - ei) X P3 +p[(e3 • P3)(ei + 62 + 63) - p^] _ 



p 



j -k 

(l/2)a(i-k) 



Now, let 



P = T^MT 



whose entries are displayed below: 

Pll = + t^]^M2t21 + t^]^M3t3i 

PI2 = t^iM2t22 + t^]^M3t32 
PI3 = t 3 iM 3 t 33 

P2I = t^2M2t21 + tg2M3t3i 
P 22 = t^2^2t22 + t^2^3t32 
P 22 , = t32M3t33 

7*31 = tggMgtgi 

Pm = tIgMgtgg 
Pm = tIgMgtgg 



Upon performing the foregoing operations, we end up with 



T^MT = p 



-{\/A)a?m {7lA)a?m —{\/2)a?m — I 

-{\/2)a?m 0 (l/4)a^m + / 

(l/2)a^m (l/4)a^m — / 0 



= P 



the second term of the above expression for I simply being P^. In 
order to compute the third term, we need the products WM and 
MW. However, it is apparent that the latter is the negative of the 
transpose of the former, and so, all we need is one of the two terms. 
Furthermore, note that since both matrices M and W are block- 
diagonal, their product is block-diagonal as well, namely. 



WM = diag(WiMi, W2M2, W3M3) 



Wi 



Vli O 

o o 



where for * = 1 , 2 , and 3 , 
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with O denoting the 3x3 zero matrix, while fij is the cross-product 
matrix of vector tUj. Moreover, 



WiMi 



mi o 
o o 



Therefore, WjMj is skew-symmetric; as a consequence, WM is also 
skew-s}mimetric, and the difference WM — MW vanishes. Hence, in 
this particular case, I reduces to 

i = p + 



That is. 



1 = p 



-(f/2)a^m (5/4)a^m —I 

(5/4)a^m 0 a?m + I 

—I {\/2)a?m 0 



{in) Now, the term of Coriolis and centrifugal forces can be computed in 
two ways, namely, (a) as (T^MT + T^WMT)0, and (b) by using 
the Newton-Euler algorithm with Oi = 0, for * = 1, 2, and 3. We 
proceed in these two ways in order to verify the correctness of our 
results. 

In proceeding with the first alternative, we already have the first term 
in the foregoing parentheses; the second term is now computed. First, 
we note that 



WMT 



WiMitii 0 0 

W2M2t21 W2M2t22 0 

WsMstai W3Mat32 W3M3t33 



with 0 defined as the 6-dimensional zero vector. The foregoing non- 
trivial 6-dimensional arrays are computed below: 



WiMitii = 

W2M2t21 = 
W2M2t22 = 

W3M3t3i = 



p/(j + k) X j 



: pi 



— 1 
0 



^3 

o 

pl{l 





k 




7r23k' 




—o\ 




0 



o 
o 

-j + k) X k 

0 



: pi 



1 -J 
0 



m,i 


o 


k 




/Oik' 


o 


o 


_-(a/2)(i + j)_ 




0 


m2 


o 


k 




7r22k 


o 


o 


_-(a/2)(2i+j) 




0 


m2 


o 


j 




1^23 


o 


o 


_-(a/2)(i + k)_ 




0 



= pi 
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\V3M3t32 



\V3M3t33 



1^3 

o 



0 


j 






0 


_-(a/2)(i + 2k)_ 




0 



pl{i 



-j + k) X j 

0 



= pi 



-i + k 
0 



m3 0 


i 




irisi 


0 0 






0 


p/(i + j + k) X i 

0 


pi 


j-kl 

0 



where 0 now denotes the 3-dimensional zero vector. Therefore, 



WMT 



pi 



0 

0 

i 

0 



1 -J 
. 0 



0 0 - 

0 0 

-i 0 

0 0 

i + k j-k 
0 0 . 



and hence, 



T^WMT 



pi 



0 1 -1 

-1 0 1 

1 -1 0 



which turns out to be skew-symmetric. Notice, however, that this will 
not always be the case. The reason why the above product turned out 
to be skew-symmetric in this example is that the individual matrices 
Wj and Mj commute, a consequence of the assumed inertial isotropy, 
which leads to the isotropy of matrices Ij, for * = 1,2, and 3. Now, 
we have 

T^MT + T^WMT = pA 

with A defined as 



A = 



— (3/4)a^m 
-(f/2)a^m — I 
(3/4)a^m + 1 



iJ IA)a?m + I 

0 

(l/4)a^m — 2/ 



-(l/2)a2m-2/ 
(l/4)a^m + 2/ 

0 



Hence, the term of Coriolis and centrifugal forces is 



(T^MT + T^WMT )0 = / 



(l/2)a^m — I 
-(f/4)a^m + I 
a?m — I 



thereby completing the desired calculations. 

Now, in order to verify the correctness of the above results, we will com- 
pute the same term using the Newton-Euler algorithm. To this end, we set 
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= 0, for * = 1, 2, and 3, in that algorithm, and calculate the desired 
expression as the torque required to produce the joint rates given above. 

Since we have already calculated the angular velocities, we will skip these 
calculations here and limit ourselves to the mass-center velocities, angular 
accelerations, and mass-center accelerations. We thus have 



Cl 



C2 



C3 



X Pi =pk X ( --a ) (i - j) 






Cl + X (ai - Pi) +(jJ 2 X P 2 

^ap[-i-j - k X (i+j) + (j +k) X (i + j - k)] = -iap(3i + j + k) 

C2 + W2 X (a2 - P2) + W3 X Pg 

-iap[3i + j + k + (j + k) x (i + k) - (i + j + k) x (2i + k)] 
-iap(3i + j + 2k) 



Now, the acceleration calculations are implemented recursively, which 
yields 



thi 

U>2 

thg 

Cl 

C2 



C3 



6»iei = 0 

UJl+ UJlX 0262 = P^k X j = -p^i 

W 2 + W 2 X 0363 = -p^i +p^(j +k) X i = -p^(i - j + k) 



thi X Pi + tui X {u>i X pi) = ap^k X 



k X ^(-i+j) 






Cl + thi X (ai - pi) + wi X [u>i X (ai - pi)] + thg x pg 
+ u !2 X (u !2 X P2) = iap2(i-j) + 0+ iap2(i+j) 

-^ap^O + 2k) + iap2(-2i - 3 j + 3k) = ^ap2(-4j + k) 

C2 + th2 X (a2 - P2) + W2 X [u >2 X (a2 - P2)] + thg X Pg 
+ Wg X (Wg X Pg) = iap2(-4j +k) - iap^j + iap^(2i - j +k) 

+ - j - 2k) + iap^(-3i + 3j) = -2ap^j 



With the foregoing values, we can now implement the inward Newton-Euler 
recursions, namely, 

kf = mgCg — f = — m(2ap^j) — 0 = — 2 amp^j 
nf = I3W3 + Wg X I3W3 - n + Pg X fg^ 

= — /p^(i — j + k) + 0 — 0 — a^mp^(— i + 2k) 

= — /p^(i — j + k) + a^mp^ii — 2k) 
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= ni2C2 + + k) - amp^j = ^amp‘^{-6j + k) 

nf = I2W2 + <^2 X I2W2 + nf + {sl 2 - P2) X + P2 X ^2 
= + 0 — Ip^{i — j + k) + —a^mp^{i — 2k) + a^mp'^i 

+ -a^mp^(— 4i — j — 6 k) 

= —Ip^{2\ — j + k) + ja^mp^( 2 i — j — 10 k) 
ff = mici+f 2 ^ = iamp^(i - j) + iamp^(- 6 j +k) 

= iamp^(i - 7j + k) 

nf = Iiwi + X Iiwi +- 0.2 + (ai - pj X + Pi X ff 
= 0 + 0 — p^I{2i — j + k) + -a^mp^ ( 2 i — j — 10 k) 

— -a^mp^(i — j — 6 k) + -a^mp^(i + j — 6 k) 

= -/p^( 2 i - j + k) + ( 2 i + j + 2 k) 

and hence, 

T3 = nf • 63 = —Ip^ + a^mp^ 

p 7-2^22 

T 2 = n 2 • 62 = Jp — —a mp 

P r 2 ^ 2 2 

Ti = • ei = —If) + —a mp 

thereby completing the calculation of the term containing Coriolis and 
centrifugal forces, i.e., 

—Ip^ + a?mp'^ 

C(6>, 6)6 = Ip^ - (l/4)a2mp2 

—Ip^ + (l/ 2 )a^mp^ 

As the reader can verify, the natural orthogonal complement and the New- 
ton-Euler algorithm produce the same result. In the process, the reader may 
have realized that when performing calculations by hand, the Newton-Euler 
algorithm is more prone to errors than the natural orthogonal complement, 
which is more systematic, for it is based on matrix-times- vector multipli- 
cations. 

6.6.2 Algorithm Complexity 

The complexity of this algorithm is analyzed with regard to the three items 
involved, namely, (*) the evaluation of L, (m) the solution of systems (6.105a 
& b), and (in) the computation of r. 
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FIGURE 6.8. Recursive calculation of vectors Vij. 

The evaluation of L involves, in turn, the three following steps: (a) the 
computation of P; (b) the computation of I; and (c) the Cholesky decom- 
position of I into the product L^L. 



(i.a) In the computation of P, it is recalled that Hj, a*, and and con- 
sequently, 5i = Sii— p^, are constant in Jv+i, which is the frame fixed 
to the *th link. Moreover, at each step of the algorithm, both revo- 
lute and prismatic pairs are considered. If the jth joint is a revolute, 
then the logical variable R is true; if this joint is prismatic, then R 
is false. Additionally, it is recalled that Oj+i, in JF^-coordinates, is 
simply the last column of Qj. The columnwise evaluation of P, with 
each pij array in JF^+i-coordinates, is described in Algorithm 6.6.1. 
Note that in this algorithm, Yij is calculated recursively from 
To do this, we use the relation between these two vectors, as displayed 
in Fig. 6.8. 

(i.b) Now we go on to the computation of I, as described in Algorithm 6.6.2. 
In that algorithm, the subscripted brackets indicate that the vectors 
inside these brackets are represented in IFk+i coordinates. 

(i.c) Because the Cholesky decomposition of a positive-definite matrix is 
a standard item, it is not discussed here. This step completes the 
computation of L. 

(ii) The solution of systems (6.105a & 6.105b) is a standard issue as well, 
and hence, needs no further discussion. 

(iii) The term r is computed using the recursive Newton-Euler formula- 
tion, as discussed in Section 6.4. To do this, we calculate r by setting 
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enddo 



enddo 
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e = 0 in that procedure, which introduces a slight simplification of 
the complexity of the inverse-d}mamics algorithm. 

Below we determine the computational complexity of each of the forego- 
ing steps. 

(i.a) This step includes Algorithm 6.6.1, which involves two nested do- 
loops. The first statement of the outermost loop involves no floating- 
point operations; the second statement involves (a) one multiplication 
of a matrix by a vector, (b) one cross product, and (c) one multipli- 
cation of a scalar by a vector. Of the last three items, (a) is done off- 
line, for the matrix and the vector factors are both constant in 
coordinates, and so, this operation is not counted. Moreover, item 
(b) is nothing but the cross product of vector [ej\j^i = [0, 0, 1]^ 
by vector Yjj. A similar operation was already discussed in connec- 
tion with Algorithm 4.1 and was found to involve zero floating-point 
operations, for the result is, simply, [e^ x Yjj ]j^i = \ —y, x, 0 ]^, with 
X and y denoting the Aij+i and Tj+i components of Yjj. Hence, item 
(b) requires no floating-point operations, while item (c) requires 2n 
multiplications and zero additions. 

The innermost do-loop, as pertaining to revolute manipulators, in- 
volves two coordinate transformations between two consecutive coor- 
dinate frames, from tFi~ to J^j+i-coordinates, plus two vector sums, 
which consumes 16(n — i) multiplications and 14(n — i) additions; 
this loop also consumes one matrix-times- vector multiplication, one 
cross product and one scalar-times- vector multiplication, which re- 
quires 18(n — i) multiplications and 12(n — i) additions. Thus, the 
total numbers of operations required by this step, for an n-revolute 
manipulator, are Mia multiplications and Aia additions, as given be- 
low: 

n 

Mia = 2n + ^ 34(n - i) = I7n^ - 15n (6.113a) 

i=l 

n 

= ^26(n-f) = 13n^ - 13n (6.113b) 

i=l 

the presence of prismatic pairs reducing the above figures. 

(i.b) This step, summarized in Algorithm 6.6.2, is also composed of two 
do-loops, each containing the inner product of two 6-dimensional ar- 
rays, and hence, requires six multiplications and five additions. More- 
over, in the outermost do-loop, this operation is performed n times, 
whereas in the innermost loop, ~ 0 times, i.e., n(n — l)/2 

times. Thus, the step requires Mu, multiplications and Au, additions, 
as given below: 

Mib = 3n^ + 3n, Aw = (6.114) 
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(i.c) This step performs the Cholesky decomposition of an n x n symmet- 
ric and positive-definite matrix, a standard operation that requires 
Mic multiplications and Aic additions (Dahlquist and Bjorck, 1974), 
namely, 

1 a 1 9 1 - 1 a 1 9 1 

Mic = —n -\ — n -\ — n, Aic = —n -\ — n -\ — n (6.115) 

6 2 3’ 6 2 3 ^ ' 

(ii) In this step, the two triangular systems of equations, eqs.(6.105a & 
b), are solved first for x and then for 9 . The numbers of operations 
it takes to solve each of the two systems, as derived by Dahlquist 
and Bjorck (1974), are repeated below for quick reference; these are 
labelled Mu and An, respectively, i.e.. 

Mu = V? , An = V? — n (6.116) 

(iii) In this step, r is computed from inverse dynamics, with w® = 0 
and 0 = 0. If this calculation is done with the Newton-Euler formu- 
lation, we then have the computational costs given in eq.(6.43), and 
reproduced below for quick reference: 

Miii = 137n - 22, Aiu = m)n-IA (6.117) 

Because of the simplifications introduced by setting the joint accelerations 
equal to zero, the foregoing figures are, in fact, slightly lower than those 
required by the general recursive Newton-Euler algorithm. 

Thus, the total numbers of multiplications and additions required for the 
forward dynamics of an n-revolute, serial manipulator are 

_ 1 o 43 o 376 , 1 o o 593 

Mf = -tA A tA A n — 22, At = -tA + ^T'tA A n — 14 (6.118) 

^6 2 3 ’^6 6 ^ ^ 

In particular, for a six-revolute manipulator, one obtains 

M/ = 1,540, = 1,227 (6.119) 

We have reduced the foregoing figures even further by introducing a mod- 
ified Denavit-Hartenberg labeling of coordinate frames and very careful 
management of the computations involved. Indeed, in (Angeles and Ma, 
1988), the complexity of the algorithm for a six-revolute manipulator of 
arbitrary architecture is reduced to 1,353 multiplications and 1,165 addi- 
tions. Since the details of this simplification lie beyond the scope of the 
book, we do not elaborate on this item here. 

6.6.3 Simulation 

The purpose of the algorithm introduced above is to enable us to pre- 
dict the behavior of a given manipulator under given initial conditions. 
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applied torques, and applied loads. The ability of predicting this behavior 
is important for several reasons: for example, in design, we want to know 
whether with a given selection of motors, the manipulator will be able to 
perform a certain typical task in a given time frame; in devising feedback 
control schemes, where stability is a major concern, the control engineer 
cannot risk a valuable piece of equipment by exposing it to untested control 
strategies. Hence, a facility capable of predicting the behavior of a robotic 
manipulator, or of a system at large, for that matter, becomes imperative. 

The procedure whereby the motion of the manipulator is determined 
from initial conditions and applied torques and loads is known as simu- 
lation. Since we start with a second-order n-dimensional system of ODE 
in the joint variables of the manipulator, we have to integrate this system 
in order to determine the time- histories of all joint variables, which are 
grouped in vector 9 . With current software available, this task has become 
routine work, the user being freed from the quite demanding task of writing 
code for integrating systems of ODE. Below we discuss a few issues pertain- 
ing to the implementation of the simulation-related algorithms available in 
commercial software packages. 

As a rule, simulation code requires that the user supply a state- variable 
model of the form of eq.(6.45), with the state- variable vector, or state- vector 
for brevity, x, and the input or control vector u defined as 







■ 0 ' 


0 







u(t) = r(t) 



(6.f20) 



With the above definitions, then we can write the state- variable equations, 
or state equations for brevity, in the form of eq.(6.45), with f(x, r) given 

by 



f(x, r) 



l(0)-i [C(0, - 5(0, iA) - 7(0)] + l(0)-ir(t) 



(6.f2i) 



thereby obtaining a system of 2n first-order ODE in the state- variable vec- 
tor X defined above. Various methods are available to solve the ensuing 
initial- value problem, all of them being based on a discretization of the 
time variable. That is, if the behavior of the system is desired in the inter- 
val to < t < tp, then the software implementing these methods provides 
approximations { yj, to the state- variable vector at a discrete set of in- 
stants, {tk }|5^, with tN =tp. 

The variety of methods available to solve the underlying initial-value 
problem can be classified into two main categories, explicit methods and 
implicit methods. The former provide yfc+i explicitly in terms of previously 
computed values. On the contrary, implicit methods provide yfc+i in terms 
of previously computed values y^,, yfc-i, . . ., etc., and yk+i itself. For ex- 
ample, in the simplest of implicit methods, namely, the backward Euler 
method, we can approximate the integral of f in the interval tk <t < tk+i 




6.6 Manipulator Forward Dynamics 267 



by resorting to the trapezoidal rule (Kahaner et ah, 1989), which leads to 
the expression 

Yfc+i = Yfc + Yfc+i) (6.122) 

In eq.(6.122), hk is the current time-step —tk and f(tfc+i,Yfc+i) can 
be an arbitrary function of Yfc+i. If this function is nonlinear in the said 
variable, then, a direct — as opposed to iterative — computation of Yfc+i is 
very unlikely. Hence, most likely an iterative scheme must be implemented 
at every integration stage of an implicit method. While this feature might 
render implicit schemes unattractive, they offer interesting advantages. In- 
deed, the iterative procedure mentioned above requires a tolerance to decide 
when and whether the procedure has converged. The convergence crite- 
rion imposed thus brings about a self-correcting effect that helps keep the 
unavoidable truncation error under control. This error is incurred when 
approximating both the time derivative x and the integral of f by floating- 
point operations. 

Current software provides routines for both implicit and explicit meth- 
ods, the user having to decide which method to invoke. Of the explicit meth- 
ods in use, by far the most common ones are the Runge-Kutta methods. Of 
these, there are several versions, depending on the number of evaluations of 
the function f(tj,yj), for various values of i, that they require. A two-stage 
Runge-Kutta method, for example, requires two function evaluations, while 
a four-stage Runge-Kutta method requires four. The self-correcting feature 
of implicit methods, not present in Runge-Kutta methods — to be sure, 
implicit Runge-Kutta methods also exist (Gear, 1971), but these are less 
common than their explicit counterparts — is compensated for by a clever 
strategy that consists in computing Yfc+i using two Runge-Kutta schemes 
of different numbers of stages. What is at stake here is the magnitude of the 
local error in computing Yfc+i, under the assumption that yj, is error- free. 
Here, the magnitude of the error is of order hP, where p is the order of the 
method in use. In Runge-Kutta methods, the order of the method is iden- 
tical to its number of stages. In general, a method is said to be of order p 
if it is capable of computing exactly the integral of an ordinary differential 
equation, provided that the solution is known to be a pth-degree polyno- 
mial. Now, upon computing yfc+i using two Runge-Kutta schemes with N 
and W + 1 stages, we can compare the two computed values reported by 
each method, namely, y^^ and y^/. If a norm of the difference of these 
two values is smaller than a user-prescribed tolerance, then the step size 
in use is acceptable. If not, then the step size is halved, and the process is 
repeated until the foregoing norm is within the said tolerance. The most 
common Runge-Kutta methods are those combining two and three stages 
and those combining four and five. 

A drawback of Runge-Kutta methods is their inability to deal with what 
are known as stiff systems, first identified by Gear (1971). As defined by 
Shampine and Gear (1979), a system of ordinary differential equations is 
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said to be stiff if it is not unstable and its linear part — i.e., the linear part 
of the series expansion of f, evaluated at the current instant — comprises a 
coefficient matrix that has an eigenvalue with a negative real part whose 
absolute value is much greater than that of the other eigenvalues. In other 
words, stiff systems of ODE are stable systems with very different time 
scales. Thus, stiff systems are not inherently difficult to integrate, but 
they require a special treatment. Gear’s method, which is implicit, pro- 
vides exactly the means to handle stiff systems. However, methods like 
Runge-Kutta’s, with excellent performance for nonstiff systems, perform 
rather poorly for stiff systems, and the other way around. The mathemat- 
ical models that arise in robotic mechanical systems are likely to be stiff 
because of the various orders of magnitude of the physical parameters in- 
volved. For example, robotic manipulators are provided, usually, with links 
close to the base that are heavy and with links far from the base that are 
light. As a consequence, when simulating robotic mechanical systems, a 
provision must be made for numerical stiffness. 

Commercial software for scientific computations offers Runge-Kutta meth- 
ods of various orders, with combinations thereof. For example, IMSL offers 
excellent FORTRAN routines, like IVPRK, for the implementation of Runge- 
Kutta methods, while Matlab’s Simulink toolbox offers the C functions 
rk23 and rk45 for the implementation of second-and-third and fourth- 
and-fifth-order Runge-Kutta methods. With regard to stiff systems, IMSL 
offers a subroutine, IVPAG, implementing both Adams’s and Gear’s meth- 
ods, while Simulink offers the adams and gear functions for the imple- 
mentation of either of these. Since Matlab is written in C, communication 
between Matlab and FORTRAN programs is not as direct as when using 
IMSL, which may be disappointing to FORTRAN users. Details on linking 
FORTRAN code with Matlab and other related issues are discussed in the 
pertinent literature (Etter, 1997). Moreover, the FORTRAN SDRIV2 sub- 
routine (Kahaner, Moler, and Nash, 1989) comprises features that allow it 
to handle both stiff and nonstiff systems. 



6.7 Incorporation of Gravity Into the Dynamics 
Equations 

Manipulators subjected to gravity fields have been discussed in Section 6.4 
in connection with the Newton-Euler algorithm and with Kane’s equa- 
tions. As found in that section, gravitational forces can be incorporated into 
the underlpng models without introducing any major modifications that 
would increase the computational load if the method of Luh et al. (1980) is 
adopted. Within this approach, gravitational forces are taken into account 
by defining the acceleration of the mass center of the 0th link, the base link, 
as equal to — g, the negative of the gravity-acceleration vector. The effect of 
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this approach is to propagate the gravity effect into all the links composing 
the manipulator. Thus, the kinematics algorithm of Section 6.4 need not 
be modified in order to include gravity forces, for all that is needed is to 
declare 

[toll ^ [-g]i (6.123) 



If inverse dynamics is computed with the natural orthogonal complement, 
then the twist-rate of the first link will have to be modified by adding a 
nonhomogeneous term to it, thereby accounting for the gravity-acceleration 
terms. That is. 






+ Si^ii + 



0 

-g 



(6.124) 



Otherwise, the foregoing algorithms require no modifications. Further- 
more, with regard to simulation, it is pointed out that the r term de- 
fined in eq.(6.104), and appearing in the right-hand side of eq.(6.105a), is 
computed from inverse dynamics with zero frictional forces and zero joint 
accelerations. 



6.8 The Modeling of Dissipative Forces 

Broadly speaking, frictional forces are of two basic types, namely, (*) vis- 
cous forces and (n) Coulomb, or dry-friction, forces. The latter occur when 
contact between two solids takes place directly, the former when contact 
between the solids takes place via a viscous fluid, e.g., a lubricant. In the 
analysis of viscous fluids, a basic assumption is that the relative velocity 
between the fluid and the solid vanishes at the fluid-solid interface, i.e., at 
the solid boundary confining the fluid. Hence, a velocity gradient appears 
within the fluid, which is responsible for the power dissipation inside it. In 
fact, not all the velocity gradient within the fluid, but only its symmetric 
part, is responsible for power dissipation; the skew- symmetric part of the 
velocity gradient accounts for a rigid-body rotation of a small fluid element. 
Thus, if a velocity field v(r, t) is defined within a region TZ occupied by a 
viscous fluid, for a point of the fluid of position vector r at a time t, then, 
the velocity gradient grad(v) = dw/dr, can be decomposed as 

grad(v) = D + W (6.125) 

where D and W are the symmetric and the skew-s}mimetric parts of the 
velocity gradient, i.e., 

D = i[grad(v) + grad^(v) ], W = ^[grad(v) — grad^(v) ] (6.126) 

The kinematic interpretation of D and W is given below: The former 
accounts for a distorsion of an infinitesimally small spherical element of 
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fluid into a three-axis ellipsoid, the ratios of the time rates of change of the 
lengths of the three axes being identical to the ratios of the real eigenvalues 
of D; the latter accounts for the angular velocity of the ellipsoid as a rigid- 
body. Clearly, both D and W change from point to point within the fluid 
and also from time to time, i.e., 

D = D(r, t), W = W(r, t) (6.127) 

Since the skew-symmetric matrix W accounts only for the rotation of a 
differential element of fluid as a rigid body, it cannot be responsible for any 
energy dissipation, and hence, the only part that is responsible for this is 
D. In fact, for a linearly viscous, incompressible fluid of viscosity coefficient 
p, the power dissipated within TZ is given by 

U^= /itr(D2)d7^ (6.128) 

Jn 

Now, if the motion of the lubricant separating the two cylindrical surfaces 
of a revolute pair is modeled as a purely tangential velocity field (Currie, 
1993), which assumes that the two cylinders remain concentric, then the 
foregoing expression for leads to the dissipation function 

A = i/302 (6.129) 

where 0 is the relative angular speed between the two cylinders and the 
coefficient /3 is a function of the lubricant viscosity and the geometry of 
the kinematic pair at hand. If the kinematic pair under study is prismatic, 
then we can model the motion of the lubricant between the two prismatic 
surfaces as a Couette flow between a pair of parallel surfaces of the sides of 
the prism. Under these conditions, then, the associated dissipation function 
A takes on the same form of that given for a revolute pair in eq.(6.129), in 
which the sole difference is that 0 changes to 6, the time rate of change of 
the associated joint variable. Of course, b is the relative speed between the 
two prismatic surfaces. Thus in any event, the dissipation function of the 
*th joint due to linearly viscous effects can be written as 

Ai = i/3i02 (6.130) 

where 0j changes to 6j if the *th pair is prismatic. The dissipation function 
thus arising then reduces to 



n ^ 

A = ^Ai = -0^B0 
1 2 ! 

where the constant n x n matrix B is given by 



(6.131) 



B = diag(/3i, /32 , . . . , /3„) 



(6.132) 
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and hence, the generalized force 5^ associated with linearly viscous effects 
is linear in the vector of joint rates, 0, i.e., 

BA 

5"^ = ^ = -Be (6.133) 

Be 

and so, A = —(1/2)11'®, which was introduced in eqs.(6.11) and (6.12a & b). 

Coulomb, or dry friction, is much more difficult to model. If denotes 
either the dissipative torque produced by Coulomb friction at a revolute 
or the dissipative force produced by Coulomb friction at a prismatic joint, 
and Oi the associated joint rate, then, the simplest model for the resulting 
generalized Coulomb-friction force is 

6f = -rf sgn(0i) (6.134) 

where sgn(-) denotes the signum function, which is defined as +1 or — 1, 
depending on whether its argument is positive or negative, and rf is a 
positive constant representing a torque for revolute joints or a force for 
prismatic joints. The numerical value of this constant is to be determined 
experimentally. The foregoing model leads to a simple expression for the 
associated dissipation function, namely, 

Af'=rf\()i\ (6.135) 

The Coulomb dissipation function for the overall manipulator is, then, 

n 

= Y,^?\6i\ (6.136) 

1 

The foregoing simplified model of Coulomb friction forces is applicable 
when the relative speed between the two surfaces in contact is high. How- 
ever, at low relative speed, that model becomes inaccurate. In robotics 
applications, where typical end-effector maximum speeds are of the order 
of 1 m/s, relative speeds are obviously low, and hence, a more accurate 
model should be introduced. Such a model should account for the empir- 
ical observation that Coulomb frictional forces are higher at low relative 
speeds and become constant at very high relative speeds. A model taking 
this fact into account has the form 

= -(rf + eje^T'*l®*l)sgn(0i) (6.137) 

where y*, and Cj are constants associated with the *th joint and are to be 
determined experimentally. The foregoing expression readily leads to the 
dissipation function associated with the same joint, namely, 

Af =rf|<i,| + ^(l-e-^*l®*l) 

7i 



(6.138) 
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and hence, the Coulomb dissipation function of the overall manipulator 
becomes 






E 



1 L 






7i 



(6.139) 



Dissipation functions are very useful. On the one hand, they allow us 
to obtain associated generalized frictional forces when these are difficult, if 
not impossible, to express in formula form. On the other hand, since dis- 
sipation functions represent nonrecoverable forms of power, their integrals 
over time yield the dissipated energy. Moreover, the energy dissipated into 
unrecoverable heat can be estimated from an energy balance, and hence, 
the parameters associated with that dissipation function can be estimated 
with suitable identification techniques, once a suitable model for a dissipa- 
tion function is available. Furthermore, the said parameters appear in the 
generalized frictional forces as well. For this reason, knowing these parame- 
ters is essential for the modeling of the corresponding generalized frictional 
forces. 




7 

Special Topics in Rigid-Body 
Kinematics 



7.1 Introduction 

The motivation for this chapter is twofold. On the one hand, the determi- 
nation of the angular velocity and angular acceleration of a rigid body from 
point- velocity measurements is a fundamental problem in kinematics. On 
the other hand, the solution of this problem is becoming increasingly rele- 
vant in the kinematics of parallel manipulators, to be studied in Chapter 8. 
Moreover, the estimation of the attitude of a rigid body from knowledge of 
the Cartesian coordinates of some of its points is sometimes accomplished 
by time-integration of the velocity data. Likewise, the use of accelerometers 
in the area of motion control readily leads to estimates of the acceleration 
of a sample of points of a rigid body, which can be used to estimate the 
angular acceleration of the body, and hence, to better control its motion. 

In order to keep the discussion at the level of fundamentals, we assume 
throughout this chapter that the information available on point velocity and 
point acceleration is error-free, a rather daring assumption, but useful for 
understanding the underlying concepts at this level. Once the fundamentals 
are well understood, devising algorithms that yield the best estimates of 
angular velocity and acceleration in the presence of noisy measurements 
becomes an easier task. For the sake of conciseness, the problem of motion 
estimation will not be discussed in this book. 
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7.2 Computation of Angular Velocity from 
Point- Velocity Data 

The twist of a rigid body, as introduced in eq.(3.74), defines completely 
the velocity field of a rigid body under arbitrary motion. Notice that the 
twist involves two vector quantities, the angular velocity and the velocity 
of a point of the rigid body. Since we are assuming that point- velocity data 
are available, the only item to be computed is the angular velocity of the 
body under study, which is the subject of this section. Once the angular 
velocity is known and the velocities of a set of body points are available, 
other relevant motion parameters, such as the location of the ISA — see 
Section 3.4 — -can be readily determined. 

If the twist of a rigid body is known, the computation of the velocity of 
an arbitrary point of the body, of a given position vector, is straightforward. 
However, the inverse problem, namely, the computation of the twist of the 
motion under study given the velocities of a set of points of known position 
vectors, is a more difficult task. A solution to this problem is now outlined. 

First and foremost, we acknowledge that the velocities of a minimum 
of three noncollinear points are needed in order to determine the angular 
velocity of the rigid body under study. Indeed, if the velocity of a single 
body point is known, we have no information on the angular motion of 
the body; if the velocities of two points are known, we can calculate two 
components of the angular- velocity vector of the body, namely, those that 
are orthogonal to the line joining the two given points, thereby leaving 
one component indeterminate, the one along that line. Therefore, in order 
to know the angular velocity of a rigid body in motion, we need at least 
the velocities of three noncollinear points of the body — obviously, knowing 
only the velocities of any number of points along one line yields no more 
information than knowing only the velocities of two points along that line. 
We thus assume henceforth that we have three noncollinear points and that 
we know perfectly their velocities. 

Let the three noncollinear points of the body under study be denoted by 
{ Pi and let { p* be their corresponding position vectors. The centroid 
C of the foregoing set has a position vector c that is the mean value of the 
three given position vectors, namely, 

c = ^^Pi (7.1) 

Likewise, if the velocities of the three points are denoted by pj, and that 
of their centroid by c, one has 




(7.2) 
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From eq.(3.51), the velocity of the three given points can be expressed as 





Pi 


= c + r2(pi-c), * 


= 1,2,3 


(7.3a) 


or 












Pi 


-c = r2(pi-c), * 


= 1,2,3 


(7.3b) 


Now, 


we define a 3 x 3 : 


matrix P as 








P 


= [Pl - C P2 - C 


P3 - C] 


(7.4) 


Upon 


differentiation of both sides of eq.(7.4) 


with respect to time. 


one has 




P 


= [Pl - C P2 - C 


P3 - c] 


(7.5) 



Thus, eqs.(7.3b) can be written in matrix form as 

P = f2P (7.6) 

from which we want to solve for fi, or equivalently, for u). This cannot be 
done by simply multiplying by the inverse of P, because the latter is a 
singular matrix. In fact, as the reader can readily verify, any vector having 
three identical components lies in the nullspace of P, thereby showing that 
P is singular, its nullspace being spanned by that vector. Furthermore, 
notice that from eq.(7.3b), it is apparent that 

(pi — c)^tu = 0, *=1,2,3 (7.7a) 

Upon assembling all three scalar equations above in one single vector equa- 
tion, we obtain 

P^w = 0 (7.7b) 

a result that is summarized below: 

Theorem 7.2.1 The angular-velocity vector lies in the nullspace of matrix 
P^, with P defined in eq.(7.5). 

In order to find the desired expression for u> from the above equation, 
we recall here a result which is proven in Appendix A: Let S be a skew- 
symmetric 3x3 matrix and A be an arbitrary 3x3 matrix. Then, 

vect(SA) = i [tr(A)l — A] vect(S) (7.8) 

Upon application of the foregoing result, eq.(7.6) leads to 

Dtu = vect(P) (7.9) 

where D is defined below and vect(ri) is nothing but tu: 
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D^i[tr(P)l-P] (7.10) 

Thus, eq.(7.9) can be solved for uj as long as D is invertible. But since the 
three points are noncollinear, D is invertible if and only if tr(P) does not 
vanish. Indeed, if tr(P) vanishes, D becomes just one-half the negative of P, 
which as we saw above, is singular. Moreover, as we saw in Example 2.6.1, 
matrix P is not frame-invariant in the sense of eq.(2.136). Thus, tr(P) is not 
frame-invariant either. However, if the three given points are noncollinear, 
then it is always possible to find a coordinate frame in which the trace 
of P does not vanish. Furthermore, under the assumption that a suitable 
coordinate frame has been chosen, the inverse of D can be proven to be 



D-i = al -/3P2 

where coefficients a and (3 are given below: 

^ ^ 4 

tr(P)’ tr(P)[tr(P2) — tr^(P)] 



(7.11) 



(7.12) 



From expressions (7.12) it is apparent that D fails to be invertible not only 
when tr(P) vanishes, but also when the term in brackets in the denominator 
of (3 does. In Exercise 7.2, the reader is confronted with a case in which the 
said term vanishes, yet the three points are not collinear. 

Prom the foregoing discussion, it is clear that given the velocities and the 
position vectors of three noncollinear points of a rigid body, the angular 
velocity of the body can always be determined. However, the data, i.e., 
the velocities of the three given points, cannot be arbitrary, for they must 
conform to eq.(7.6) or to Theorem 7.2.1. Equation (7.6) states that the 
columns of matrix P must lie in the range of while Theorem 7.2.1 states 
that u) lies in the nullspace of P. However, prior to the computation of tu, or 
equivalently, of it is not possible to verify this condition. An alternative 
approach to verifying the compatibility of the data follows: Since lines PiC 
belong to a rigid body, vectors p* — c must remain of the same magnitude 
throughout a rigid-body motion. Moreover, the angles between any two of 
the said lines must be preserved throughout the motion as well. This means 
that the conditions below must hold: 



(Pi - c)^(Pj - c) = Cij, i,j = 1,2,3 (7.13) 

or in compact form, 

P^P = C (7.14) 

where the (*, j) entry of the constant matrix C is Cjj, as defined in eq.(7.13) 
above. Upon differentiation of both sides of eq.(7.14) with respect to time, 
we obtain 
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Theorem 7.2.2 (Velocity Compatibility) The velocities of three points 
of a rigid body satisfy the compatibility condition given below: 

P^P + P^P = 0 (7.15) 

vhth matrices P and P defined in eqs.(7.f) and (7.5) and O denoting the 
3x3 zero matrix. 

The above equation, then, states that for the given velocities of three 
points of a rigid body to be compatible, the product P^P must be skew- 
symmetric. Note that the above matrix compatibility equation represents 
six independent scalar equations that the data of the problem at hand must 
satisfy. There is a tendency to neglect the foregoing six independent scalar 
compatibility conditions and to focus only on the three scalar conditions 
drawn from the diagonal entries of the above matrix equation. This is, 
however, a mistake, for these three conditions do not suffice to guarantee 
data compatibility in this context; all these three conditions guarantee is 
that the distance between any pair of points of the set remains constant, 
but they say nothing about the angles between the pairs of lines formed by 
each pair of points. 

Note, on the other hand, that the product PP^ has no direct geometric 
interpretation, although the difference tr(PP^)l — PP^ does, as discussed 
in Exercise 7.9. Furthermore, while Theorem 7.2.2 states that matrix P^P 
is skew-symmetric, it says nothing about the product PP^. All we can say 
about this product is stated in the result below: 

Theorem 7.2.3 With matrices P and P defined in eqs.(7.f) and (7.5), 
the product PP^ obeys the constraint 

tr(PP^) = 0 (7.16) 



If m X n matrices are regarded as forming a vector space, then an inner- 
product of two such matrices A and B, denoted by (A, B), can be defined 
as 

(A, B) = tr(AB^) (7.17) 

the two matrices being said to be orthogonal when the foregoing inner 
product vanishes. We thus have that Theorem 7.2.3 states that matrices 
P and P are orthogonal, a result that parallels that about the orthogo- 
nality of the relative velocity of two points and the line joining them, as 
stated in eq.(3.53) and summarized in the ensuing theorem. The proof of 
Theorem 7.2.3 is left as an exercise. 

Example 7.2.1 The rigid cube shown in Fig. 7.1 moves in such a way 
that vertices Pi, P 2 , and P 3 undergo the velocities shown in that figure, for 
three different possible motions. The length of the sides of the cube is 1, and 
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P, 

(a) (b) (c) 



FIGURE 7.1. A rigid cube undergoing a motion determined by the velocities of 
three of its points. 

the velocities all have magnitude a/2 in Figs. 7 . 1 a and c; these velocities 
are of unit magnitude in Fig. 7. 1 b. Furthermore, in the motion depicted in 
Fig. 7 . 1 c, the velocity of is parallel to line P4P3, whereas that of P2 is 
parallel to line P1P3. Out of the three different motions, it is known that 
at least one is compatible. Identify the compatible motion and compute its 
angular velocity. 

Solution: Let pj denote the velocity of Pi, of position vector p*. Each pro- 
posed motion is then analyzed: (a) The projection of pi onto P1P2 is 1 , 
but that of p2 onto the same line is 0 , and hence, this motion is incom- 
patible; (b) Again, the projection of pi onto P1P2 is 1 , but that of p2 
onto the same line vanishes, and hence, this motion is also incompatible. 
Thus, the only possibility is (c), which is now analyzed more formally: Use 
a dextrous — right-handed — rectangular coordinate frame with origin at Pi , 
axis Y along P1P2, and axis Z parallel to P2P3- All vectors and matrices 
are now represented in this coordinate frame, and hence. 




Thus, 



0 

2 

2 




7.2 Computation of Angular Velocity from Point- Velocity Data 279 



Now matrices P and P are constructed: 



P 



Furthermore, 



■ 0 


0 


o ' 




CO 


0 


1 

CO 


-2 


1 


1 


1 


1 


-2 


-1 


-1 


1 

CM 




1 

1 

to 


1 


1 



rr ■ 1 

p^p = - 

9 



0-3 3 

3 0-3 

-3 3 0 



which is skew-symmetric, and hence, the motion is compatible. Now, matrix 
D is computed: 



D^l[ltr(P)-P] = i 



3 0 0 

2 2-1 
1 1 1 



The angular velocity u> is computed as the solution to 

Dtu = vect(P) 



where 

vect(P) = i 

D 

Equations (7.2) are thus 

2iuji = 3 

2lui + 2uj2 — uj^ = —1 
UJl + UJ2 + tos = 1 

The first of the foregoing equations leads to 

LUl = I 



3 

-1 

1 



whereas the second and the third lead to 



2uj2 — u>2, = —3 
= 0 



and hence, 

UJ2 = — 1 , ‘^3 = 1 

Now, as a verification, u> should be normal to the three columns of P as 
defined in eq.(7.15); in other words, u> should lie in the nullspace of P^. 
But this is so, because 



'3 1-2' 

0 1 1 




1 ■ 
-1 


1 

o 


1 

0 O 

1 


-3 -2 1 




1 


6 


[oj 



thereby verifying that u> lies, in fact, in the nullspace of P"^. 
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7.3 Computation of Angular Acceleration from 
Point- Acceleration Data 

The angular acceleration of a rigid body under general motion is determined 
in this section from knowledge of the position, velocity, and acceleration 
vectors of three noncollinear points of the body. The underlying procedure 
parallels that of Section 7.2. Indeed, recalling the notation introduced in 
that section, and letting vectors pj, for * = 1, 2, 3, denote the acceleration 
of the given points, one can rewrite eq.(3.87) for each point in the form 

Pi = c + ($7 + r2^)(pi - c), *=1,2,3 (7.18a) 

or 

Pi - c = ($7 + r2^)(pi - c), *=1,2,3 (7.18b) 

where c was defined in eq.(7.1), and c is the acceleration of the centroid, 
i.e., 

1 ® 

c = -^Pi (7.18c) 

1 

Furthermore, matrix P is defined as 

P=[pi-C P2-C P3-c] (7.19) 

Thus, eqs.(7.18b) can be written in compact form as 

P = {h + n^)p (7.20) 

from which one is interested in computing $7, or correspondingly, u>. To 
this end, eq.(7.20) is rewritten as 

f2P = W (7.21a) 

with matrix W defined as 

W = P - (7.21b) 

The counterpart of Theorem 7.2.1 is now derived from eqs.(7.18b). First, 
these equations are cast in the form 

Pi - c - r2^(pi - c) = th X (pi - c), * = 1, 2, 3 

It is now apparent that if we dot-multiply the above equations by u), we 
obtain 

[pi — c — r2^(pi — c)] • th = 0, * = 1, 2, 3 (7.22a) 

Upon assembling the three foregoing equations in one single vector equa- 
tion, we derive the counterpart of eq.(7.7b), namely, 

(P - n'^Pfui = 0 (7.22b) 



a result that is summarized below in theorem form: 
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Theorem 7 . 3.1 The angular- acceleration vector u) lies in the nullspace of 
matrix 'W'^ , with W defined in eq. (7.21b). 

Just as we did in Section 7.2 when solving for uj from eq.(7.9), we apply 
the result already invoked in connection with eq.(7.9), thereby deriving an 
alternative form of eq. (7.21a), namely, 

Dth = vect(P — (7.23) 

where D is defined as in eq.(7.10). Thus, 

th = D^^vect(P - n^P) (7.24) 

with given as in eqs.(7.11) and (7.12). As in Section 7.2, then, given 
the position, velocity, and acceleration vectors of three noncollinear points 
of a rigid body, it is always possible to compute the associated angular 
acceleration. However, as discussed in that section, the data cannot be 
given arbitrarily, for they must comply with eq. (7.21a), or correspondingly, 
with eq.(7.22b). The former implies that the three columns of matrix W he 
in the range of matrix $7; alternatively, eq. (7.22b) implies that $7 lies in the 
nullspace of W^. Again, prior to the determination of f2, it is impossible 
to verify this condition, for which reason an alternative approach is taken 
to verif}4ng compatibility. The obvious one is to differentiate both sides of 
eq.(7.15), which produces 

P^P + 2P^P + P^P = 0 (7.25) 

thereby deriving the compatibility conditions that the acceleration measure- 
ments should satisfy. 

Finally, upon differentiation of both sides of eq.(7.16) with respect to 
time, and while doing this, resorting to Lemma A. 2 of Appendix A, we 
have 

tr(PP^ + PP^) = 0 (7.26) 

which is the counterpart of eq.(7.16 ). 



Example 7 . 3.1 The three vertices of the equilateral triangular plate of 
Fig. 7.2, which lies in the X-Y plane, are labeled Pi, P2, and P3, their 
position vectors being pi, p2, and P3. Moreover, the velocities of the fore- 
going points are denoted by pj, for * = 1,2, 3. The origin of the coordinate 
frame X, Y, Z lies at the centroid C of the triangle, the velocities of the 
vertices, in this coordinate frame, being given as 



4 - a/2 


'O' 


4 - a/3 


'O' 


4 + V2 


'o' 


4 


0 

1 


, P2= 4 


0 

1 


, P 3 = 4 


0 

1 



Likewise, pi, p2, and p3 denote the accelerations of the three vertices of 
the plate, given below in the same coordinate frame: 

8a/3 + 3a/6' 

3V3 
0 



Pi 



1 



-6 + 4a/3 
12 -3a,^ 
0 



P2 



1 
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FIGURE 7.2. A rigid triangular plate undergoing a motion given by the velocity 
and acceleration of its vertices. 



6 + 4a/3 
-12 + 3a/2 
0 

With the foregoing information, 

(a) show that the three given velocities are compatible; 

(b) compute the angular velocity of the plate; 

(c) determine the set of points of the plate that undergo a velocity of 
minimum magnitude; 

(d) show that the given accelerations are compatible; 

(e) compute the angular acceleration of the plate. 



P3 



1 



Solution: 

(a) Since the centroid of the triangle coincides with that of the three 
given points, we have c = 0. Moreover, 



1/2 




0 




■ -1/2 ■ 


-V3/6 


, P2 = 


V3/3 


, P3 = 


-V3/6 


0 




0 




0 



1 






6 



3 0-3 

a/3 2a/3 -a/3 

0 0 0 



Thus, 




7.3 Computation of Angular Acceleration from Point-Acceleration Data 283 



Furthermore, 



and hence, 




0 

0 

V3)/12 



1 






12 



0 

0 

a/3 - 3a/2 



0 0 

0 0 

2a/3 a/3 + 3a/2 



We can readily show from the above results that 

P^P = O 



with O denoting the 3x3 zero matrix. Hence, matrix P^P is skew- 
symmetric and the velocities are compatible 

(b) Next, we have 



and 



D^-[tr(P)l-P] 



1 

12 



2a/3 0 

A^ 3 
0 0 



3 

V3 

3 + 2a/3 



vect(P) 



1 



-2a/3 
a/3 + 3a/2 
0 



Hence, if the components of oj in the given coordinate frame are 
denoted by ct;*, for * = 1, 2, 3, then we obtain 



2\/ZijJi + 3ct;3 
a/3ci;i + 3ct;2 + 



-V3 

-a/3 + 3a/2 
2 



(3 + 2V3)c^3 = 0 



From the third equation, 

uj?, = 0 



Substitution of the foregoing value into the first of the above equa- 
tions yields cji = —1/2. Further, upon substitution of the values of cji 
and into the second of the above equations, we obtain uj 2 = %/2/2 
and hence. 






1 

2 
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(c) Let pg be the position vector of the point Pg on the instantaneous 
screw axis lying closest to the origin. Now, in order to find pg, we 
can resort to eq.(3.72), using point C* as a reference, i.e., with c and c 
playing the roles of a and a in that equation. Moreover, since c = 0, 
the expression for pg reduces to 



Po 



1 

hF 



Lie 



where from item (b). 



while 



Lie 






3 

4 



12- a/3 
24 



■V2‘ 

1 

0 



and hence. 



Po 



12 - a/3 
18 



V2' 

1 

0 



As a verification, pg should be perpendicular to the ISA, as it is, 
for the product w^Pg to vanish. Next, the vector representing the 
direction of the screw axis is obtained simply as 



e 



UJ 

M 




V2 of 



thereby defining completely the instant screw axis. 



(d) The acceleration of the centroid of the three given points is given as 
follows: 



c 



_ aF ()]T 

24 ’ 24 ’ ^ 



Then, matrices P, P^P, P^P, and P^P are readily computed as 



pTp 



pTp 



■-6 + 4a/3 + a/6 


1 

00 

1 

I\5 

Fh 


6 + 4a/3 + a/6 


12-3a/2 + a/3 


-2a/3 


-12 + 3a/2 + a/3 


0 


0 


0 


-21 + 6 a /6 


6-24a/3-6a/6 


15 + 24a/3 


6 + 24a/3 - 6a/6 


-12 


6 - 24 a/3 + 6a/6 


15 - 24 a/3 


6 + 24a/3 + 6a/6 


-21 -6a/6 


-21 + 6 a /6 


6 + 24a/3-6a/6 


15 - 24a/3 


6-24a/3-6a/6 


-12 


6 + 24 a/3 + 6a/6 


15 + 24 a/3 


6-24a/3 + 6a/6 


-21 -6a/6 
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pTp 



1 

144 



21 - 6 a /6 
—6 + 6 a /6 
-15 



6 + 6 a /6 -15 

12 - 6 - 6 a /6 

6 - 6 a /6 21 + 6 a /6 



Now, it is a simple matter to verify that 



P^P + 2P^P + P^P = O 

and hence, the given accelerations are compatible. 

(e) is defined as the unique skew-symmetric matrix whose vector is tu, 
the latter having been computed in item (b). Thus, 



0 


0 V2l 


, = - 


-2 


-V2 0 1 


0 


0 


1 


-V2 


-1 


0 


_-V2 


-1 


0 


4 


0 


0 


-3 






1 



-6 + a /6 - 2 a /6 6 + a /6 

3a/2 + a/3 -2a/3 3a/2 + a/3 

0 0 0 



Hence, 



P - n^P 



1 



4a/3 

12 

0 



8a/3 4a/3 
0 -12 

0 0 



The angular- acceleration vector is thus computed from 



Dth = vect(P — fi^P) 



where D was computed in item (b), while 



vect(P — fi^P) 



1 

12 



3 

V3 

3 + 2a/3 



and hence, letting chj denote the *th component of 6 j in the given 
coordinate frame, we obtain 



12 



(2a/3c5i + 3ch3 



— (a/3c5i + 3(1j2 + V^uJs) — 

/(3 + 2v/3)o,3 = 12Ai! 



12 
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which yields 



6j 



0 

0 

1 



thereby completing the solution. Note that 6j lies, in fact, in the 
nullspace of matrix (P — 
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Kinematics of Complex Robotic 
Mechanical Systems 



8.1 Introduction 

Current robotic mechanical systems, encountered not only in research lab- 
oratories but also in production or construction environments, include fea- 
tures that deserve a chapter apart. Generically, we will call here complex 
robotic mechanical systems all such systems that do not fall in the cate- 
gory of those studied in Chapter 4. Thus, the complex systems at hand are 
manipulators either of (*) the serial type that do not allow a decoupling of 
the positioning and the orientation problems, or (m) of the parallel type, 
besides other systems than manipulators, such as dextrous hands, walking 
machines, and rolling robots. While redundant manipulators of the serial 
type fall within this category as well, we will leave these aside, for their 
redundancy resolution calls for a more specialized background than what 
we have either assumed or given here. 

A special feature of serial manipulators of the kind studied here is that 
they can admit up to sixteen inverse kinematics solutions. Such manipula- 
tors are now in operation in industry, an example of which is the TELBOT 
System, shown in Fig. 8.f, which features all its six motors on its base, the 
motion and force transmission taking place via concentric tubes and bevel 
gears. This special feature allows TELBOT to have unlimited angular dis- 
placements at its joints, no cables traveling through its structure and no 
deadload on its links by virtue of the motors (Walischmiller and Li, f996). 
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FIGURE 8.1. The TELBOT System (Courtesy of Walischmiller GmbH, Meers- 
burg, Germany.) 

8.2 The IKP of General Six- Revolute Manipulators 

As shown in Chapter 4, the IKP of six-revolute manipulators of the most 
general type leads to a system of six independent equations in six unknowns. 
This is a highly nonlinear algebraic system whose solution posed a chal- 
lenge to kinematicians for about two decades and that was not considered 
essentially solved until the late eighties. Below we give a short historical 
account of this problem. 

Pieper (1968) reported what is probably the earliest attempt to formu- 
late the inverse kinematic problem of six-axis serial manipulators in a uni- 
variate polynomial form. He showed that decoupled manipulators, studied 
in Section 4.4, and a few others, allow a closed-form solution of their in- 
verse kinematics. However, apart from the simple architectures identified by 
Pieper, and others that have been identified more recently (Mavroidis and 
Roth, 1992), a six-axis manipulator does not admit a closed-form solution. 
Attempts to derive the minimal diMracteristic polynomial for this manip- 
ulator were reported by Duffy and Derby (1979), Duffy and Crane (1980), 
Albala (1982), and Alizade et al. (1983), who derived a 32nd-degree poly- 
nomial, but suspected that this pol}momial was not minimal, in the sense 
that the manipulator at hand might not be able to admit up to 32 postures 
for a given set of joint variables. Tsai and Morgan (1985) used a technique 
known as polynomial continuation (Morgan, 1987) to solve numerically the 
nonlinear displacement equations, cast in the form of a system of quadratic 
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equations. These researchers found that no more than 16 solutions were to 
be expected. Briefly stated, polynomial continuation consists basically of 
two stages, namely, reducing first the given problem to a system of poly- 
nomial equations; in the second stage, a continuous path, also known as a 
homotopy in mathematics, is defined with a real parameter t that can be 
regarded as time. The continuous path takes the system of equations from 
a given initial state to a final one. The initial state is so chosen that all 
solutions to the nonlinear system in this state are either apparent or much 
easier to And numerically than those of the originally proposed system. 
The Anal state of the system is the actual system to be solved. The initial 
system is thus deformed continuously into the Anal state upon varying its 
set of parameters, as t varies from 0 to 1. At each continuation step, a set 
of initial guesses for each of the solutions already exists, for it is simply the 
solution to the previous continuation step. Moreover, flnding the solutions 
at the current continuation step is done using a standard Newton method 
(Dahlquist and Bjorck, 1974). 

Primrose (1986) proved conclusively that the problem under discussion 
admits at most 16 solutions, while Lee and Liang (1988) showed that the 
same problem leads to a 16th-degree univariate polynomial. Using diflerent 
elimination procedures, as described in Subsection 8.2.3 below, Li^ (1990) 
and Raghavan and Roth (1990, 1993) devised diflerent procedures for the 
computation of the coefficients of the univariate polynomial. While the 
inverse kinematics problem can be considered basically solved, research on 
flnding all its solutions safely and quickly still continued into the nineties 
(Angeles et ah, 1993). Below we describe two approaches to solving this 
problem: first, the bivariate-equation approach, introduced in (Angeles and 
Etemadi Zanganeh, 1992) is described; then, the methods of Raghavan 
and Roth (1990, 1993) and of Li (1990), aimed at reducing the kinematic 
relations to a single univariate polynomial, are described. 



8.2.1 Preliminaries 



We start by recalling a few definitions that were introduced in Chapter 4. 
In Section 4.2 we introduced the matrices Qj and the vectors a* defining 
the coordinate transformations from frame to frame Pi or, equiva- 

lently, the displacement of the former to the latter. The 4x4 homogeneous 
matrix — see Section 2.5 — transforming coordinates in Pi^i to coordinates 
in Pi is given by 



Ai 



0 ^ 1 



( 8 . 1 ) 



^N. B. Lee and Li of the references in this chapter are one and the same 
person, namely, Dr.-lng. Hongyou Lee (a.k.a. Dr.-lng. Hongyou Li). 
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where 0 is the 3-dimensional zero vector, while the 3x3 rotation matrix 
Qj and the 3-dimensional vector a* were defined in Chapter 4 as 



Cj 


Si 


fliSi 




0,i Ci 


Si 




Ci 


, 3-i — 


0,i Si 


0 




Aj 







In the above definitions we used the Denavit-Hartenberg notation, whereby 
is the distance — and hence, a* > 0 — between the Zi~ and the Zj+i-axes, 

while hi is the offset oo < hi < +oo — between the Xi~ and Xj+i-axes, 

as measured along the positive direction of the Zj-axis. Moreover, 



A,' = cos a,' 



yUj = sm a.i 



where Oi is the *th joint angle, measured between Xi and Xj+i in the 
positive direction of Zj, and Oj denotes the twist angle between Zi and 
Zj+i, for * = 1, . . . , 6. Furthermore, the factoring of matrix Qj, introduced 
in eq.(4.2a), is reproduced below for quick reference: 



Qi — ZjXj 

where Xj and Zj denote two pure reflections, namely, 
Xj = 

xr = 



■f 


0 


0 ■ 




Cj 


Si 


0l 


0 


-Aj 




, Zj = 


Si 


-Ci 0 


1 

O 




Aj_ 




0 


0 


IJ 



x; 



-1 



r-l 



(8.3) 

(8.4a) 

(8.4b) 



the foregoing reflections thus being both symmetric and self-inverse — see 
Section 2.4. As a consequence. 



Qf = XjZj 



We will also use the partitionings of Qj displayed in eq.(4.12), namely. 



Qi = [ Pi qi Uj 



(8.5) 



A quick comparison between eqs.(8.2) and (8.5) leads to the relations below: 



Ci 




Si 




■ 0 ■ 


'^iSi 


, rij = 


^i C-j 


, Oi = 




fliSi 




f-^iC-i 







We also have 

Ui = Qie=[piSi -piCi Aj]^, e=[0 0 1]^ (8.7a) 

bj Qj cij , cij Qjbj , bj = [ cii hiPi hiXi ] (8.7b) 
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Moreover, using eqs.(8.4a) and (8.7b), we can write 

bj = Xjgj, gi = [ai 0 bif' (8.8) 

From eqs.(8.3), (8.4a-b), (8.7b), and (8.8), we have 

gj = Zjaj = Xjbj (8.9) 



Furthermore, vector Xj, of eq.(4.11), is reproduced below for quick refer- 
ence: 



Xi 



cos Oi 
sin Oi 



( 8 . 10 ) 



A useful concept in this context is that of bilinear form: An algebraic 
expression of the form Auv, where u and v are two given scalar variables and 
A is a constant, is said to be bilinear in u and v. Likewise, an expression of 
the form Av?v“^ is said to be biquadratic in u and u, with similar definitions 
for bicubic, trilinear, and multilinear forms. Moreover, the same definitions 
apply to vector and matrix expressions, as pertaining to their components 
and, correspondingly, their scalar entries. Now we have 



Lemma 8.2.1 Let matrix A be skew- symmetric and B be defined as the 
similarity transformation of A given below: 

B = QiAQf (8.11) 



where Qj was defined in eq.(f.le) and A is assumed to be independent of 
Oi. Then, B is linear in Xj . 

Proof: This result follows from relation (2.138). Indeed, as the reader can 
readily verify, B is skew-symmetric as well, and the product Bv, for any 
3-dimensional vector v, can be expressed in terms of b, defined as vect(B) — 
see Section 2.3.3. That is, 

Bv = b X V 

If a denotes vect(A), then a and b, by virtue of eq.(8.11) and the results 
of Section 2.6, obey the relation 



b = Qja 



Hence, 

Bv = (Qja) X V 

thereby showing that the resulting product is linear in Xj, q.e.d. 
Moreover, let 



Tj = tan 



(8.12a) 



which allows us to write the identities below, as suggested by Li (1990): 






(8.12b) 



— 1 
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h 




FIGURE 8.2. Partitioning of the manipulator loop into two subloops. 

We now define p as the vector directed from the origin of T\ to the 
operation point (OP) P of Fig. 8.2. Moreover, we let 1 = [l^, ly, IzV' , 
m = [nix, 'rriy, 'm-z Y , and n = [ux, Uy, Uz Y represent the three mutually 
perpendicular unit vectors parallel to the X 7 , I 7 and Z-j axes, respectively, 
of JF 7 , which has its origin at P — a layout of these axes is depicted in Fig. 4.3 
for a decoupled manipulator. Flence, the pose of the EE is described in the 
base frame Pi, by means of the homogeneous transformation A given as 



Q 

0 ^ 


p 

1 


, Q = [1 m n] = 


1 1 


nix 

niy 


1 








mz 


nz \ 



In the next step, we derive a set of scalar equations in five unknowns, upon 
eliminating one of these, that is fundamental in computing the solution of 
the problem at hand. 

Derivation of the Fundamental Closure Equations 

Given the geometric parameters of the manipulator and the pose of the EE 
with respect to the base frame, we derive the displacement equations of 
the manipulator, a.k.a. the loop-closure equations, from which all unknown 
angles are to be computed. We start by recalling the (matrix) rotation 
and (vector) translation equations of the general six-axis manipulator, as 
displayed in eqs.(4.9a & b), and reproduced below for quick reference: 
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Q 1 Q 2 Q 3 Q 4 Q 5 Q 6 — Q ( 8 . 13 a) 

+ Qia 2 + QiQ 2 a 3 + . . . + QiQ 2 Q 3 Q 4 Q 5 ae = P ( 8 . 13 b) 

The use of 4 x 4 homogeneous transformations in the ensuing preparatory 
work will ease the suitable recasting of the foregoing equations. Thus, by 
using the matrices Aj of eq.( 8 . 1 ) in the above rotation and translation 
equations, we end up with a 4 x 4 matrix equation, namely, 

A 1 A 2 A 3 A 4 A 5 A 6 = A (8-14) 

The unknown variables in the above equations are the joint angles 
the IKP thus consists in solving the closure equations for these unknowns. 
Equation (8.14) — or, equivalently, eqs.(8.13a) and (8.13b) — comprises 12 
scalar equations and four identities; however, among these equations, only 
six are independent, for the columns (or the rows) of a rotation matrix must 
form an orthonormal — mutually orthogonal and of unit magnitude — set of 
vectors. The orthonormality property of the columns or rows of a rotation 
matrix, thus, brings about six scalar constraints. 

The basic approach to solving the IKP resorts to disassembling the kine- 
matic chain of the manipulator at two joints, e.g., joints 3 and 6 , to obtain 
two subchains or subloops (Li et al., 1991). The first subchain, as suggested 
in the foregoing reference, and depicted in Fig. 8.2, goes from joint 3 to 
joint 6 via joints 4 and 5, while the second subchain goes from joint 6 to 
joint 3 via the EE and joints 1 and 2. Algebraically, this is equivalent to 
rewriting eq.(8.14) in the form 



A3 A4 A,^ 



A^-'Ar'AAg-i 



(8.15a) 



Note that eq.(8.15a), in component form, leads to 



12i((^3t(^A,(^5) 122((^3t(^A,(^5) 123((^3t (^A, (^5) 12a((^3t (^A, (^5) 

hl{(^A,0b) l32{(^A,(^5) h3{(^A,0b) ^34(^^4, ^^s) 

0 0 0 1 . 

rii(6'i, 6*2, ^'e) ri2(6»i, 6»2, 6»e) ri3(6»i,6»2) ri4(6»i,6»2) 

?'2l(^^l, ^^2, ^^e) r22{0i,92,0fj) r23(<?l,<?2) ?'24(^^l, ^^ 2 ) 

?'3l(^^lj ^^2, ^^e) r 32 { 0 i, O 2 , Oq) r33(0i,02) ?'34(^^1, ^^ 2 ) 

0 0 0 1 



(8.15b) 



where kj and rij denote nontrivial components of the left- and the right- 
hand sides, respectively, of eq.(8.15a). Note that, because of the forms of 
matrices Qj, whose third rows are independent of 9i, the third row of the 
left-hand side of eq.(8.15a) or, equivalently, of eq.(8.15b), is independent of 



' 3 - 
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It should be apparent that other pairs of joints can be used to disassemble 
the kinematic chain of the manipulator into two subchains; what matters 
is that none of the two subchains contain more than three joints; else, the 
entries of the homogeneous matrices become unnecessarily complex on one 
side of the matrix equation, while the entries of the other side become 
unnecessarily simple. 

Now we extract one rotation and one translation equation from the 4x4 
matrix equation (8.15a), namely, 

Q3Q4Q5 = Q^QrQQl’ (8.16a) 

Q3(b3 + Q4b4 + Q4Q5b5) = Q^Qf(p - Qbe) - (b2 + Q^bi) (8.16b) 



which are kinematically equivalent to eqs.(8.13a & b), but algebraically 
much simpler. Note that, in eq.(8.16b), we used eq.(8.7b) to substitute 
aj by Qjbj. Upon multiplying both sides of eq.(8.16a) from the right by e 
and using eqs.(8.7a), we obtain 

Q3Q4U5 = , o-g = Qoe (8.17) 

On the other hand, eq.(8.16b) can be cast in the form 

Qs(b3 + Q4b4 + Q4Q5b5) = Q^Qfp - (b2 + Q^bi) (8.18) 



where p = p -Qbg. Both sides of eq.(8.17) represent a unit vector parallel 
to Zg, in frame JCg. Similarly, the left- and the right-hand sides of eq.(8.18) 
represent the vector directed from the origin of JCg to the origin of IFq, 
in frame JCg. By adopting the notation of Tsai and Morgan (1985), also 
adopted by Raghavan and Roth (1990, 1993), we define four vectors that 
will play a key role in the ensuing derivations, namely. 



f = f(<?4,<?5) 



h = h(6»i) = 



fi 

/2 

h 



hi 

h2 

hs 



f = f(6»4,6»5) 



ri 

r-2 

r3 



n = n(<?i 



ni 

«-2 

"-3 



— X3(ba + Q4b4 + Q4Q5b5) 



Qfp-bi 



X 3 Q 4 U 5 



Qf^-g 



(8.19a) 

(8.19b) 

(8.19c) 

(8.19d) 



Expressions for the components of the above four vectors are given in Table 
8.1, where 7* (* = 1, 2, 3), p, q, r, m , v, and w are auxiliary variables. Using 
eqs.(8.19a-d), (8.3), (8.4b), (8.8), and (8.9), we can rewrite eqs.(8.17) and 
(8.18) in terms of the foregoing vectors, namely. 



Z3f(<?4,<?5) =X2[Z2h(0i) -g2] 
Z3f(04,<?5) = X2Z2n(<?i) 



(8.20a) 

(8.20b) 
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Item 


Expression 


Item 


Expression 


h 


C4fi'l + S45'2 + C*3 


ri 


C4mi + S4m2 


/2 


—^3{si9i — C 492 ) 
+ 9393 


r-2 


-A3(s4mi - C4m2) 
+ P3«r3 


h 


93{s491 - C 492 ) 
+ ^393 + bs 


r3 


^ 3 ( 54^1 - C4m2) 
+ A3m3 


91 


C5C*5 + «4 


mi 


S 595 


92 


— 35 X 4^05 + P 465 


ni2 


C 5 A 4 P 5 + M 4 A 5 


93 


S5M405 + A 465 + 64 


m 3 


— C 5 P 4 P 5 + A 4 A 5 


hi 


Cip + Siq - ai 


ni 


ClM + SlV 


h2 


-Ai(sip - ciq) 
+ pi(r - di) 


«-2 


—Xi{siu — civ) 
+ PlW 


hs 


9i{sip - ciq) 
+ Ai(r - di) 


"-3 


Pl{siU — Clv) 
+ Aiw 


P 


-\-Px 


u 


'^xpQ 


9 


-^Py 


V 




r 


{'^zp6 ’^z^6)^6 

-^Pz 


w 





TABLE 8.1. Expressions for the components of vectors f, h, f, and n 



These six scalar equations play a key role in deriving the fundamental clo- 
sure equations in five unknowns that are needed to solve the problem at 
hand. We describe first a straightforward — albeit not the computationally 
most suitable — method that we call the direct method, to derive the equa- 
tions of interest. 

Four vectors are defined in terms of the two sides of each of eqs. (8.20a 
& b), namely, 



f = f(<? 3 , <? 4 , 0 s) — Q 3 (b 3 + Q 4 b 4 + Q 4 Q 5 b 5 ) — 0 s) ( 8 . 21 a) 

g = g(6»i,6»2) = Q^Qfp- (b2 + Q^bi) 

= (Qfp - bi) - b2 = (Qf p - bi) - X2g2 
= X 2 [Z 2 h( 0 i) -g 2 ] ( 8 . 21 b) 

h = h(6»3, 6»4, 6»5) = Q 3 Q 4 U 5 = Z3f(6»4, 9 ^,) ( 8 . 21 c) 

i = i(6»i, 92) = = X2Z2ii(6»i) ( 8 . 21 d) 

where we made use of eq.( 8 . 8 ). Further, in light of definitions (8.17), it 
is apparent that f and g are, in fact, the third columns, excluding their 
bottom entries, of the left- and the right-hand side matrices of eq.(8.15b). 
Likewise, h and i are the fourth columns of the same matrices. Vectors g 
and i are thus free of 9q. 

Now, the six scalar equations (8.20a & b) are expressed in terms of f, g. 




296 



8. Kinematics of Complex Robotic Mechanical Systems 



h, and i, namely, 



f = g or 



h = i or 



fx{03, di, 9fl) 

/y(6»3,6»4,6»5) 


— 


gx{9i, O 2 ) 


(8.22a) 


fz{94,95) 




> 

to 




64 , 63 ) 
^4, ^ 5 ) 





*a:(^l, ^ 2 ) 
*y(^l, ^ 2 ) 


(8.22b) 


^z(^4,^s) 




_*z(^l, ^ 2 ) _ 





It should be noted that h and i are both unit vectors. Thus, each side of 
eq. (8.22b) is subjected to a quadratic constraint, i.e.. 



h h = 1, i i = 1 



and hence, out of the above six scalar equations, only five are indepen- 
dent. However, the number of unknowns in these six equations is also five. 
Therefore, eqs. (8.22a) and (8.22b) suffice to determine the five unknown 
joint angles contained therein. 

We can now create eight new equations with the same power products^ 
as f, g, h and i. The new equations are constructed using the products 
below: 



f • f = g • g 


(8.22c) 


f h = g i 


(8.22d) 


f X h = g X i 


(8.22e) 


'•h)f = (g-g)i-2(g-i)g 


(8.22f) 



It is noteworthy that eq.(8.22f) is derived by first equating the reflection? 
of vector h onto a plane normal to f with its counterpart, namely, the 
reflection of vector i onto a plane normal to g. The final form of eq.(8.22f) 
is obtained upon clearing denominators in the foregoing reflection equation. 

Equations (8.22a-f) amount to 14 scalar equations in five unknown joint 
variables {di}i- These are the fundamental closure equations sought. Some 
facts pertaining to the degree of the two sides of eqs.(8.22c-f) are proven 
below: 



Fact 8.2.1 The inner products f -f and f-h are both free ofii.^ and bilinear 
in { Xj I®, while their counterparts g • g and g • i are linear in Xi. 

Proof: 



^By power product we mean terms with their coefficients deleted; for example, 
the power products of the polynomial 5:Py + 3xz + -f 4z = 0 are the terms 
x^y, xz, y‘^ and z. 

^Neither Li nor Raghavan and Roth disclosed the geometric interpretation of 
this fourth equation, first proposed by Lee and Liang (1988). 
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f • f = ||Q3(b3 + Q4b4 + Q4Q5b5)|P 
= I|b3 + Q4b4 + Q4Q5b5p 
5 

= X] l|bdP + 2b3 Q4(b4 + Qsbs) + 2hjQ^h^ 

3 

whose rightmost- hand side is clearly free of X3 and is bilinear in {xj}®. 
Similarly, 



f h = (b3 + Q4b4 + Q4Q5b5)^Q3 Q3Q4U5 
= bg Q4U5 + bj U5 + b^ U5 

whose rightmost- hand side is clearly bilinear in X4 and X5, except for the 
last term, which contains two factors that are linear in X5, and hence, can 
be suspected to be quadratic. However, Qsbs is, in fact, as, while U5 is the 
last column of Qs, the suspicious term thus reducing to a constant, namely, 
65 cos 05 . Similar proofs for g • g and g • i will be given presently. Moreover, 

Fact 8 . 2.2 Vector f x h trilinear in { Xj while its counterpart, g x i, 
is bilinear m { Xj }^. 

Proof: If we want the cross product of two vectors in frame A but have 
these vectors in frame B, then we can proceed in two ways: either (*) 
transform each of the two vectors into M-coordinates and perform the cross 
product of the two transformed vectors; or (n) perform the product of the 
two vectors in B-coordinates and transform the product vector into A- 
coordinates. Obviously, the two products will be the same, which allows us 
to write 

f X h = Q3 [b3 X (Q4U5) + (Q4b4) X (Q4U5) + (Q 4 Q 5 b 5 ) X (Q4U5)] 

= Q3{b3 X (Q4U5) + Q4(b4 X U5) + Q4 [(Qsbs) x U5)]} 

whose rightmost- hand side is apparently trilinear in {xj }g, except for the 
term in brackets, which looks quadratic in X5 . A quick calculation, however, 
reveals that this term is, in fact, linear in X5. Indeed, from the definitions 
given in eqs.(4.3c & d) and (8.5) we have 



(Qsbs) X U5 = as X U5 = 



a5^5«5 + &5M5C5 
-asAscs + 65/4555 
—05/45 



which is obviously linear in X5 . The proof for the counterpart product, g x i, 
parallels the foregoing proof, and will be given below. 

Fact 8.2.3 Vector (f • f)h — 2(f • h)f is trilinear in { Xj its counterpart, 
(g • g)i — 2(g • i)g, being bilinear in { Xj . 
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Proof: First, we write the (elongated or contracted) reflection of vector h 
in the form 

(f f)h - 2(f h)f = Q3V 

where 

V = llbilpj Q4U5 - 2[(u^Q4b3)b3 + (u^b4)b3 + (u^b4)Q4b4 

+ (u^ Q5b5)b3 + (u^ Q5b5)Q4b4 + (u^ Q5b5)Q4Q5b5] + 2w 

with all terms on the right-hand side, except for w, which will be defined 
presently, clearly bilinear in X4 and X5 . Vector w is defined as 

W = [ ]l + [ ]2 + [ Is 

each of the foregoing brackets being expanded below: 

[ = [(b|’Q4b4)Q4U5 - (u^Qlb3)Q4b4] 

= Q4(usb4 Q 4 - b4U^ Ql)b3 
= Q 4 (usb 4 - b 4 U^)Q 4 bs 

which thus reduces to a product including the factor QiAQf , with A being 
the term in parentheses in the rightmost- hand side of the last equation. This 
is obviously a skew-symmetric matrix, and Lemma 8.2.1 applies, i.e., the 
rightmost- hand side of the last equation is linear in X4. This term is, hence, 
bilinear in X4 and X5. Furthermore, 

[ ]2 = [(blQ5b5)Q4U5 - (u^b4)Q4Q5b5] 

= Q4 [(b^Q^b4)u5 - (u^b4)Q5b5] 

= Q4(u5b^Q^ - Q5b5U^)b4 

which is clearly linear in X4, but it is not obvious that it is also linear 
in X5. To show that the latter linearity also holds, we can proceed in two 
ways. First, note that the term in parentheses is the skew-symmetric matrix 
usa^ — asu^, whose vector, as x U5, was already proven to be linear in X5. 
Since the vector of a skew-symmetric matrix fully defines that matrix — see 
Section 2.3 — the linearity of the foregoing term in X5 follows immediately. 
Alternatively, we can expand the aforementioned difference, thereby deriv- 
ing 



T T 

usag -asUg 



0 

— asMs 



0 

a5^5«5 + bzUzCh 



— asAscs + 
— asAsss — 

0 



which is clearly linear in X5 . Moreover, its vector can be readily identified 
as as X Us, as calculated above. Finally, 
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[ la = [(ba Q4Q5b5)Q4U5 - (u^Qlb3)Q4Q5b5] 

= Q4(u5b^Q^ - Q5b5U^)Q4b3 
= Q4(u5a^ - a5U^)Q4 b3 

this bracket thus reducing to a product including the factor QiAQf , with 
A skew-symmetric. Hence, the foregoing expression is linear in X4, ac- 
cording to Lemma 8.2.1. Moreover, the matrix in parentheses was already 
proven to be linear in X5, thereby completing the proof for vector (f • f)h — 
2(f • h)f . The proof for vector (g • g)i — 2(g • i)g parallels the foregoing proof 
and will be given presently. 

Finally, we have one more result that will be used presently: 

Fact 8.2.4 If a scalar, vector, or matrix equation is linear iny^i, then upon 
substitution ofci and Si by their equivalent forms in terms ofxi = tan(0j/2), 
the foregoing equation becomes quadratic in Ti. 

Proof: We shall show that this result holds for a scalar equation, with 
the extension to vector and matrix equations following directly. The scalar 
equation under discussion takes on the general form 

Aci + Bsi + (7 = 0 

where the coefficients A, B, and C do not contain dj. Upon substituting 
Cj and Si in terms of Tj = tan(dj/2), and multipl}dng both sides of that 
equation by 1 + rf , we obtain 

A{l-T^) + 2BTi + C{l+T^) = 0 

which is clearly quadratic in Tj, q.e.d. 

The foregoing proof follows immediately for vector and matrix equations. 
Moreover, if a scalar, vector, or matrix equation is of degree A: in Xj, upon 
introducing the same trigonometric substitution, the said equation becomes 
of degree 2k in Tj. 

Expressions for the right-hand sides of eqs.(8.22c-d) are given below: 

2 

g • g = ^ llbjf + llpf - 2p^Qi(Q2b2 + bi) + 2bfQ2b2 (8.23a) 
1 

g i = QiQ2b2 - Qibi) (8.23b) 

g X i = (p X o-g) - b 2 X QjQja-e - QJ (bi x Qf erg) (8.23c) 

(g-g)i - 2(g-i)g= l^^llbjf + ||pf j Q^Qfo-g 

-2[(o-|’p)(Q^QrP -b2 - Q^bi) + (cr)rQiQ2b2)b2 
+(o-|’Qibi)b2 + (cr)rQibi)Q^b2] + 2w' (8.23d) 
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In deriving and simplifying the above relations, we use the invariance 
relations — -see Section 2.7 — of the dot and cross products, i.e., for any 
arbitrary vectors u and v, we have 

(Qju)^(Qjv) = u^v 
(Qiu) X (Qjv) = Qi(u X v) 



All the terms on the right-hand sides of eqs.(8.23a-d), except for w', are 
clearly bilinear in xi and X2. This bilinearity also holds for the last term 
in eq.(8.23d), i.e., w', which can be expressed in the form 

w'^[ ]; + [ ]'+[ ]' (8.24) 



Each of the above brackets is given as 



[ ]i = [(o-i’QiQ2b2)Q^Qfp - (pQiQ2b2)Q^QfcT6] 
= - o-l p){CllCll)h2 

[ ]; = [(bfQ2b2)Q^qr^6 - (^^QlQ2b2)Q^bi] 

= Q^[(Qro-6)bi -bi(QfcT6)^]Q2b2 
[ la = [(o’i’Qibi)Q^Qfp - (p^Qibi)Q^QfcT6] 

= Q^[Q^ - o-6p^)Qi]bi 



(8.25a) 

(8.25b) 

(8.25c) 



According to Lemma 8.2.1, the terms in the right-hand sides of relations 
(8.25a-c) are all bilinear in xi and X2. 

It is noteworthy that the third components of vectors f x h and (f • f)h — 
2(f -h)!, as well as f -f and f -h, are all free of O^,. Hence, among the 14 scalar 
equations, i.e., eqs.(8.22a-f), six are free of O^,. Casting all 14 equations in 
vector form results in the fundamental closure equations: 



PX45 = Rxi 2 



(8.26) 



where P and R are 14 x 9 and 14 x 8 matrices, respectively. Moreover, 
the entries of P are linear in X3, while those of R are independent of the 
joint angles. In addition, the 9- and 8-dimensional vectors X45 and X12 are 
defined as 



X45 = [S4S5 S4C5 C4S5 C4C5 S4 C4 S5 C5 1] (8.27a) 

X12 = [siS 2 S1C2 C1S2 C1C2 Si Cl S2 02 ]^ (8.27b) 

Various approaches have been reported to solve the fundamental clo- 
sure equations for the unknown joint angles, but all methods fall into two 
categories: (*) purely numerical approaches, whereby no attempt is made 
to reduce the number of unknowns (Angeles, 1985), or the reduction is 
rather limited, from six to four unknowns (Tsai and Morgan, 1985); and 
(n) elimination approaches, whereby unknowns are eliminated until a re- 
duced number of equations in a reduced number of unknowns is derived. 
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We focus here only in the second category. Of these, we have essentially 
two classes: (a) the bivariate- equation approach and (b) the univariate- 
polynomial approach. As the names indicate, the former aims at reducing 
all fundamental closure equations to a system of equations in only two un- 
knowns; the latter, in turn, aims at reducing the fundamental equations 
to one single equation in one unknown. Moreover, that single equation, 
being polynomial in nature, is termed the characteristic polynomial of the 
problem at hand. The polynomial is derived upon substituting the cosine 
and sine functions of the unknown angle, say 0^, by (1 — T^)/(l + T^) and 
2T/(1 + T^), respectively, with T = tan(0j;/2). This transformation is well 
known as the ton- /la// trigonometric identities. 

The transformation of the original problem given in terms of trigono- 
metric functions of the unknown angles into a polynomial equation in T is 
essential from a conceptual viewpoint, for this transformation makes appar- 
ent that the problem under study admits an integer number of solutions, 
namely, the degree of the characteristic polynomial. On the other hand, 
the same transformation is not trouble-free. Indeed, the mapping from 9^ 
into T apparently includes a singularity at 9^ = tt, whereby T oo. The 
outcome is that, if one of the solutions is 9^ = tt, then the characteristic 
polynomial admits at least one solution at infinity, which is reflected in a 
deflation of the polynomial. This phenomenon was made apparent in Ex- 
ample 4.4.3, where a quartic characteristic polynomial appeared as cubic 
because of one solution at infinity. The untold analyst may thus be misled 
to believing that, in the presence of a solution at infinity, the IKP at hand 
admits a smaller number of solutions than it actually does. Furthermore, 
in the neighborhood of 9^ = tt, one of the solutions is extremely large 
in absolute value, which thus gives rise to numerical inaccuracies, gener- 
ically referred to as ill-conditioning. As a matter of fact, the problem of 
polynomial-root finding has been identified as ill-conditioned by numerical 
analysts for some time (Forsythe, 1970). 

In order to cope with the foregoing shortcomings of the tan-half iden- 
tities, the author and his team devised an alternative means to solving 
the problem at hand and other similar ones in computational kinematics 
(Angeles and Etemadi Zanganeh, 1992a, b). In this approach, termed here 
the bivariate- equation approach, the 14 equations are reduced to a system 
of three bivariate trigonometric equations in the sines and cosines of two 
of the unknown angles. These equations are then plotted in the plane of 
the two unknowns, thus obtaining three contours, whose intersections yield 
the real values of the two unknowns. As a matter of fact, only two such 
equations would suffice; however, it turns out that the underlying reduc- 
tion cannot be accomplished without the introduction of spurious roots, 
which thus have to be eliminated. The spurious solutions are graphically 
identified as points of intersection of the contours where only two of these 
intersect. Actual solutions are those where the three contours intersect. 

Now, to derive the bivariate equations, we have to eliminate three of the 
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five unknowns from the 14 fundamental closure equations. The elimination 
takes place in two steps: first, the 14 equations are reduced to six equations 
free of 0\ and O 2 ] then, the foregoing six equations are reduced to three 
bivariate equations in 0^ and 0s, i.e., free of O^,. We describe the elimination 
procedure below. 

8.2.2 The Bivariate- EqvMion Approaeh 

The simplest means of eliminating 9i and O 2 is brute force. In this vein, we 
use eight of the 14 scalar equations of eq.(8.26) to solve for X 12 in terms of 
<? 3 , Oi and 0^. To do so, we partition the foregoing equation into two groups 
of six and eight equations, namely, 

P„X 45 = R„xi 2 (8.28a) 

P;X 45 = R;Xi 2 (8.28b) 

where P„ and P; are 6x9 and 8x9 submatrices of P. Likewise, R„ and 
Ri are 6 x 8 and 8 x 8 submatrices of R. In the above partitioning, the 
equations must be grouped such that R; be nonsingular. Using eqs. (8.28a 
& b), six scalar equations free of 0\ and O 2 can be derived, namely, 

Px45 = Oe , r = P„ - R„(R; ^P;) (8.29) 

where Oe is the 6 -dimensional zero vector. Since the entries of the 6x9 
matrix P are all linear in X 3 , the entry in the *th row and jth column of 
the foregoing matrix, pij , can be expressed in the form 

jij = aijCs + hjSs + Cij ] *=1,...,6; j = l,...,9 (8.30) 

In the above relation, coefficients aij, bij, and Cij are independent of the 
joint variables. Using eq.(8.30), we can expand eq.(8.29) and then rearrange 
the terms in the *th equation, thus obtaining 

AiC^ + BiS^ + Uj = 0 ; * = 1, . . . , 6 (8.31a) 

where, for * = 1 , . . . , 6 , we have 

Ai = (2^45455 T U42S4C5 T U43C4S5 T U44C4C5 T (245^4 T 

+C*i7S5 + fliSCs + 0*9 (8.31b) 

Bi = 6jlS4S5 + bi2S4C5 + &J3C4S5 + &i4C4C5 + 6*554 + 6*6C4 

+ 6*755 + 6 jgC 5 + big (8.31c) 

Ci = 0*45455 + 0*25405 + 0*30455 + 0*40405 + 0*554 + 0*504 

+ 0*755 + o*g 05 + 0*9 (8.31d) 



Moreover, let 



y* = [o* 5 * 1]^, * = 3, 4, 5 



(8.32) 
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Now the six scalar equations (8.31a) are cast in vector form as 

Dya = Oe (8.33a) 

In the above equation, D is a 6 x 3 matrix whose entries are bilinear in X4 
and X5. 

Note that, if the terms in eq. (8.31a) are exchanged, one can readily come 
up with other forms of eq.(8.33a), namely, 

Dy4 = Oe (8.33b) 

Dye = Oe (8.33c) 

where D and D are both 6x3 matrices whose entries are bilinear in (X3, X5) 
and (x3, X4), respectively. Hence, any one of eqs. (8.33a, b, or c) can be used 
to eliminate one unknown joint variable, 9^, <?4, or 9^, respectively, thereby 
deriving the bivariate equations. 

In order to derive the bivariate equations, we choose eq.(8.33a), from 
which we eliminate 9^. Indeed, from definition (8.32), y3 ^ 0, and hence, D 
must be rank-deficient, which means that every one of its 20 — the number 
of combinations of six objects taking three at a time — 3 x 3 determinants, 
obtained by picking up three of its six rows, should vanish. We need, in 
principle, only two of these determinants to obtain two independent equa- 
tions in <?4 and 9^. Actually, to be on the safe side, we should impose the 
vanishing of all 20 possible determinants, which would yield, correspond- 
ingly, 20 contours in the 94-9^ plane; the intersections of all contours would 
then yield the real {94, 9^) pairs of values which render D rank-deficient. 
Nevertheless, the visualization of the intersections of all 20 contours would 
be physically impossible, and so, we have to compromise with a smaller 
number. As we have experienced, two of the above determinants are prone 
to yield spurious solutions, for which reason we pick up three such deter- 
minants and derive three equations in 94 and 9^. Each of these equations 
describes a contour C*, for * = 1,2, 3, in the 94-9^ plane, 

Ci:Fii94,95)=0, *=1,2,3. (8.34) 

Note that, by plotting the three contours in the — tt < 9i < n region, for 
* = 4, 5 , we ensure that no real solutions will be missed. 

The intersection points can be detected visually by the user or, automat- 
ically, by a suitable vision software. Depending on the method that is used 
to detect the intersection points, a numerical nonlinear solution code can be 
employed to refine each solution to the desired accuracy. In this way, two of 
the unknown joint angles, 94 and 9^, are already solved for, the remaining 
four unknowns being determined uniquely, as described presently. Before 
discussing the computation of the remaining joint angles, we describe the 
univariate-polynomial approach, as proposed by Raghavan and Roth and 
by Li, Woernle and Hiller. 
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8.2.3 The Univariate- Polynomial Approaeh 

We describe here two procedures leading to one single univariate 16th- 
degree polynomial equation, which is the characteristic polynomial of the 
IKP at hand. The two procedures bear many similarities, but they also 
involve remarkable differences that warrant separate discussions. 

The Raghavan and Roth Procedure 

A sophisticated elimination procedure was proposed by Raghavan and Roth 
(1990; 1993). Their procedure is based on eqs.(8.20a & b), but their 14 
closure equations are different, as explained below. 

At the outset, both sides of eqs. (8.20a & b) are multiplied from the left 
by = X .2 = X 2 ; then, the two equations thus resulting are rearranged 
in the forms 



X2Zaf + g2 — Z 2 I 1 (8.35) 

X2Zaf = Z2n (8.36) 

Now, four vectors, the counterparts of those introduced in eq. (8.21a), are 
defined as 



f = X2Z3f + g2 = X2(Z3f + b2) 


(8.37a) 


g = Z2h 


(8.37b) 


h = X2Z3? 


(8.37c) 


i = Z2n 


(8.37d) 



Note that the first two components of f and h are trilinear in X3, X4, 
and X5, while their third components are bilinear in X4 and X5, and free 
of <? 3 . On the other hand, the first two components of g and i are bilinear 
in xi and X 2 , while their third components are linear in xi and free of <? 2 - 
Similar to the direct method, six scalar equations are obtained as 



f = g (8.38a) 

h = i (8.38b) 

Moreover, eight more scalar equations are obtained in the forms 

f-f = g-g (8.38c) 

f-h = g-i (8.38d) 

f X h = g X i (8.38e) 

(f • f)h - 2(f • h)f = (g • g)i - 2(g • i)g (8.38f) 



The third components of the two vectors on the right-hand sides of eqs.(8.38e 
& f), and the terms on the right-hand sides of eqs. (8.38c & d) are free of 
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O 2 and linear in xi. As proven by Raghavan and Roth in the above ref- 
erences, the eight foregoing equations have the same power products as f, 
h, g, and i. Now, Raghavan and Roth’s 14 fundamental closure equations, 
eqs.(8.38a-f), are cast in the form 

PX 45 = Rxi 2 (8.39) 



where P and R are 14 x 9 and 14 x 8 matrices, respectively. Moreover, the 
entries of P are linear in X3, while those of R are independent of the joint 
angles. Furthermore, R has the structure below: 
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0 


0 


X 


X 


_0 


0 


0 


0 


X 


X 


0 


0 . 



In the above display, all nonzero entries are denoted by x and rows are 
written according to the order of appearance in the 14 fundamental scalar 
equations, i.e., in the order of eqs.(8.38a-f). This special structure of matrix 
R is then exploited to eliminate the joint angles 0\ and O 2 in an efficient 
way. 

Based on the structure of R, Raghavan and Roth define two groups of 
six and eight equations in the form 



P„X 45 = Cxi (8.41a) 

P;X 45 = Axi 2 (8.41b) 

where C is a 6 x 2 constant matrix that is formed by the nonzero entries in 
rows 3, 6 , 7, 8 , 11, and 14 of matrix R. A is, in turn, an 8 x 6 matrix whose 
entries are all functions of the data, while X45 was defined in eq. (8.27a), 
and X 12 is the 6 -dimensional vector defined as 

Xi2 = [siS2 S1C2 C1S2 C1C2 «2 C2]^ (8.42) 

Any two of the six scalar equations in eq. (8.41a) can now be used to solve 
for xi, the resulting expression then being substituted into the remaining 
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four equations of the same group. This is done by first partitioning the six 
scalar equations as 



C„xi = d„ (8.43a) 

C;Xi = d; (8.43b) 

where C„ and C; are 2x2 and 4x2 submatrices of C, respectively, with 
d„ and d; being the corresponding 2- and 4-dimensional vectors that result 
from P„X45. If eq. (8.43a) is solved for xi and the result is substituted into 
eq.(8.43b), we obtain four equations free of 0\ and namely, 

r4X45 = QC-M„-d, = 04 , r4 = QC-i(P„)2-(P„)4 (8.44a) 

in which P4 is a 4 x 9 matrix whose entries are linear in X3, while (P«)2 
and (P„)4 are 2x9 and 4x9 submatrices of matrix P„, respectively. The 
above set of equations is now cast in the form 

DiYs = O 4 (8.44b) 

with Di defined as a 4 x 3 matrix whose entries are bilinear in X4 and X5, 
while O4 is the 4-dimensional zero vector, and ya was defined generically 
in eq.(8.32). In the partitioning of C, C„ is chosen with nonzero entries in 
the third and sixth rows of matrix R, i.e.. 



jj^ip -piq 
piu —piv 



Hence, C„ ^ is readily obtained as 



C 



-1 

u 



1 

pi{uq -pv) 



—V q 
—u p 



(8.45a) 



(8.45b) 



In general, C„ must be chosen such that it is nonsingular, which may 
require a reordering of eqs(8.41a). 

Additional equations free of 0\ and O 2 can be derived from any six of the 
eight equations in eq.(8.41b), which can be used to solve for X12; the expres- 
sions thus resulting are then substituted into the remaining two equations. 
In this way, two additional equations free of 0\ and O 2 would be obtained. 
However, this elimination process is not suitable for symbolic computations. 
Instead, Raghavan and Roth (1990) derived the two additional equations in 
a terser form. This is done by finding two independent linear combinations 
of the eight equations (8.41b) that render identically zero all terms in 0\ 
and <?2- The left-hand sides of these equations are given as 






^[(f . f)h^ - 2 (f . h)/J - + ^62! , 

Za\ Za\ a\ 

-Ai/ii(f X h)j; + piwfy - jj,i{r - bi)hy 



(8.46a) 
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ue,,, 04, 0,) = |^[(f • - 2(f • h)/J - AiMl(f X h), 

2 2 

-niwf^ + /4i(r - bi)h.^ + —S 2 fy - T^Sihy (8.46b) 

ai ^ 2ai 

while the right-hand sides are 

V’l = T^[(I • I)g - 2(g • i)gj - + — 529 .^ 

Za\ Za\ a\ 

-Ai/ri(g X i)j; + giWQy - Hi{r - hi)iy (8.46c) 

u? - - 

V>2 = ^[(g • t)iy - 2(1 • i)gy] - AiMid X i)y 

- g? _ g? - 

-giwg^ + gi{r - 6i)*j, + —hgy ~ TT^hiy (8.46d) 

ai " Zai 

On the other hand, h^, ix, f x 'dx represent the first components of 
vectors h, i, f, and g, respectively, the other components being defined 
likewise. Furthermore, Raghavan and Roth derive (5i and 82 as 

61 = + {r - hif - al 

62 = pu + qv + (r — bi)w 

Upon substitution of g and i, as given by eqs. (8.37b & d), respectively, into 
eqs. (8.46c & d), and introduction of the definitions given in Table 8.1, it 
turns out that both -01 and -02 vanish identically, i.e.. 



'tpi = 0 and "02 = 0 

Also note that, in deriving expressions (8.46a & b) and (8.46c & d), we 
assume that ai ^ 0. However, ai vanishes in many cases, the foregoing 
procedure thus becoming inapplicable. One way of coping with this case 
is to go one step behind Raghavan and Roth’s procedure and redefine, for 
k=l,2, 

4>k{S3,Si,6z) < — ai<))fc(03, 04, 0s); 

and 

'tl^k < — aiV’fc 



i.e., 

MOs, 64 , 05) = ^ [(f • f)hx - 2(f • h)7j - ^S{hx + ^S2Jx 
Z Z ai 

-aiXigi(i X h)a, + aigiwfy - aigi{r - bi)hy 



(8.47a) 
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6»4, 6»5) = y[(f • i)hy - 2(f • h)fy] - aiAi/ii(f x h)^ - ai^wf^ 



Ml 



+ ai/ii(r - + jJ,iS2fy - 



(8.47b) 



u? - - P? - 

V’l = y[(g • g)*® - 2 (g • i)3^] - y 

-aiAi/ii(g X i)j; + aipiwg - aipi{r - bi)iy 



(8.48a) 



V>2 



= y[(g • S)iy - 2 (g • i)My] - aiAiMi(g x i)y 
-aipiwg^ + aipi{r - bi)ix + glhOy ~ ^Siiy 



(8.48b) 



Under their new definitions, apparently, tjji and 'tp 2 also vanish. Once (f)i 
and (f >2 are equated to zero, two equations are obtained that can be cast in 
the form 



T2X45 — O 2 



(8.49) 



or equivalently. 



D2Y3 — O 2 



(8.50) 



where O 2 is the 2-dimensional zero vector, F 2 is a 2 x 9 matrix whose entries 
are linear in X 3 , and D 2 is a 2 x 3 matrix whose entries are bilinear in X 4 
and X 5 . 

The two eqs. (8.44a) and (8.49) thus involve a total of six scalar equations 
free of 9i and 62 , and can be combined to yield a system of six equations 
trilinear in X 3 , X 4 , and X 5 , namely. 



SX45 — Oe 



(8.51a) 



where S is a 6 x 9 matrix whose entries are linear in X 3 , and Oe is the 6 - 
dimensional zero vector. Now, the tan-half trigonometric identities relating 
Si and Cj with Tj = tan(0j/2), for * = 4, 5, are substituted into eq.(8.51a). 
Upon multiplying the two sides of the equation thus resulting by (1 +t| )(1 + 
t|), Raghavan and Roth obtained 

= Og (8.51b) 

where S' is a 6 x 9 matrix that is linear in X 3 , while x^g is defined as 

X 45 = tI T4t| T 4 T 5 T4 t| T 5 1]^ 

If the same trigonometric identities, for * = 3, are now substituted into 
eq.(8.51b), and then the first four scalar equations of this set are multiplied 
by (1+t|) to clear denominators, the equation thus resulting takes the form 

S"x(j 5 = Og 



(8.51c) 
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In the above equations, S" is a 6 x 9 matrix whose first four rows are 
quadratic in T3, while its last two rows are apparently rational functions 
of T3. However, as reported by Raghavan and Roth, the determinant of 
any 6x6 submatrix of S" is, in fact, an 8th-degree polynomial in T3 
and not a rational function of the same. Moreover, in order to eliminate 
T4 and T5, they resort to dialytic elimination (Salmon, 1964), introduced 
in this book in Subsection 4.5.3 and in Exercise 4.13. This elimination 
procedure is further discussed in Subsection 8.2.3, in connection with the 
Li, Woernle, and Hiller method, and in Section 8.3 in connection with 
parallel manipulators. 

In applying dialytic elimination, the two sides of the system of equations 
appearing in eq. (8.51c) are first multiplied by T4; then, the system of equa- 
tions thus obtained is adjoined to the original system, thereby deriving a 
system of 12 linear homogeneous equations in X45, namely, 

SX45 = O12 ( 8 . 51 d) 

where O12 is the 12-dimensional zero vector, while the 12-dimensional vector 
X45 is defined as 

X45 = T'4'^5 tI t|t| t|t5 t| T4t| T4T5 T4 t| T5 1]^ 

(8.51e) 

Furthermore, the 12 x 12 matrix S is defined as 




its 6 X 12 blocks G and K taking on the forms 

G=[S" 063], K=[063 S"] 

with Oe3 defined as the 6x3 zero matrix. 

Now, in order for eq.(8.51d) to admit a nontrivial solution, the determi- 
nant of its coefficient matrix must vanish, i.e., 

det(S)=0 (8.52) 

The determinant of S is the characteristic equation sought. This determi- 
nant turns out to be a 16th-degree polynomial in T3. Moreover, the roots 
of this polynomial give the values of T3 corresponding to the 16 solutions of 
the IKP. It should be noted that, using the same procedure, one can also 
derive this polynomial in terms of either T4 or T5 if the associated vector in 
eq.(8.51d) is written as X35 or X34, respectively. Consequently, the entries 
of matrix S would be linear in either X4 or X5. 

The Li, Woernle, and Hiller Procedure 

At the outset, the factoring of Qj given in eq.(4.1c) and the identities first 
used by Li (1990), namely, eqs.(8.12b), are recalled. Additionally, Li defines 
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a matrix Tj as 



Hence, 



T 



-Tj 1 0 

1 Tj 0 
0 0 1 



TjCj = Uj 



Tj 1 0 

1 -Tj 0 
0 0 1 



with Cj defined in eq.(4.1b). Furthermore, we note that the left-hand sides 
of the four vector equations (8.22a, b, e & f) are of the form Q3V, where 
V is a 3-dimensional vector independent of 9^. Upon multiplication of the 
above-mentioned equations from the left by matrix T3, Li and co-authors 
obtained a new set of equations, namely. 





U3i 


f = T3g 


(8.53a) 




U 3 f = T 3 i 


(8.53b) 




U 3 (f X f 


) = T3(g X i) 


(8.53c) 


U 3 


(f -f)r- 2 (f -h)f 


= T 3 [(g • g)i - 2(g • i)g] 


(8.53d) 


where f and r are defined as 

f = A 3 (b 3 + Q4b4 + Q4Q5b5) 


(8.54) 




f = A3(Q4U5) 


(8.55) 



with Aj defined, in turn, in eq.(4.1c). 

Because of the form of matrices T 3 and U 3 , the third of each of the 
four vector equations (8.53a-d) is identical to its counterpart appearing 
in eqs. (8.38a, b, e & f). That is, if we denote by either Vi or (v)j the *th 
component of any 3-dimensional vector v, the unchanged equations are 



h = 93 ( 8.56a) 

f -3 = *3 (8.56b) 

(f X r )3 = (g X i )3 (8.56c) 

(f • f)f3 - 2(f • h)/3 = (g • g)*3 - 2(g • i)gs (8.56d) 



all of which are free of 9^. Furthermore, six additional equations linear in 
T 3 will be derived by multiplying both sides of eqs.(8.56a-d) and of (8.22c 
& d) by T 3 , i.e.. 



T~3fs — T~393 
Tsh = 7-3*3 



(8.57a) 

(8.57b) 
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T3(f X f)g = T3(g X i)a (8.57c) 

T3[(f • f)rg - 2 (f • h)/3] = T3[(g • g)*3 - 2 (g • i)grg] (8.57(1) 

Tg(f • f) = Tg(g • g) (8.57c) 

Tg(f • h) = Tg(g • i) (8.571) 



We have now 20 scalar equations that are linear in Tg, namely, the 12 
eqs.(8.53a-d), plus the six equations (8.57a-f) and the two scalar equations 
(8.22c & d). Moreover, the left-hand sides of the foregoing 20 equations are 
trilinear in Tg, X 4 , and xg, while their right-hand sides are trilinear in Tg, 
xi, and Xg. These 20 equations can thus be written in the form 

Ax = (3 (8.58a) 

where the 20 x 16 matrix A is a function of the data only, while the 20- 
dimensional vector /3 is trilinear in Tg, xi, and Xg, the 16-dimensional vector 
X being defined, in turn, as 

X = [T3C4C5 T3C4S5 T3S4C5 T3S4S5 T3C4 T3S4 T3C5 T3S5 

C4C5 C4S5 S4C5 S4S5 C4 S4 Cg Sg]^ ( 8 . 58 b) 



Next, matrix A and vector (3 are partitioned as 



A = 




f^u 

/3l 



(8.59) 



where Agj is a nonsingular 16 x 16 matrix, ATisa4xl6 matrix, vector 
f3jj is 16-dimensional, and vector f3j^ is 4-dimensional. Moreover, the two 
foregoing matrices are functions of the data only. Thus, we can solve for x 
from the first 16 equations of eq. (8.58a) in the form 

X = Ay^/3y 



Upon substituting the foregoing value of x into the four remaining equa- 
tions of eq. (8.58a), we derive four equations free of x, namely, 

AiAyl/Sgg = /3^ (8.60) 

In eq.(8.60) the two matrices involved are functions of the data only, 
while the two vectors are trilinear in Tg, xi, and Xg. These equations are 
now cast in the form 



(TljCg + BjSg + Ci)r^ + 14jCg + T'jSg + f) — 0, i — 1, 2, 3, 4 (8.61a) 

where all coefficients Aj , . . . ,Fi are linear in xi. Next, Li and co-authors 
substitute cg and sg in the foregoing equations by their equivalents in terms 
of Tg = tan(0g/2), thereby obtaining, for * = 1, 2, 3, 4, 



CiiT^T^ + 2BjTgT3 + AjjTg + FuT^ + 2UjTg + Du — 0 



(8.61b) 
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with the definitions 



A-ii = Ai Ci 


(8.61c) 


Cii = Ci — Ai 


(8.61d) 


Du = Di -\- Fi 


(8.61e) 


1 

III 


(8.61f) 



Further, T2 and T3 are both eliminated dialytically from the four equations 
(8.61a). To this end, both sides of all four equations (8.61b) are multiplied 
by T2 , which yields 



CiiT^T^ + 2BjT|T3 + AjjT2T3 + FuT^ + 2FjT| + DuT2 = 0 (8.61g) 

We have now eight equations that are linear homogeneous in the 8- 
dimensional nonzero vector z defined as 

z=['^ 2*'^3 T-f T2T3 t| T 3 T2 1]^ (8.61h) 

and hence, the foregoing 8-dimensional system of equations takes on the 
form 

Mz = 0 (8.62) 

where the 8x8 matrix M is simply 





- 0 


Cn 


0 


2Bi 


Fn 


All 


2Ei 


Dll 




0 


C22 


0 


2B2 


F22 


A22 


2E2 


D22 




0 


C33 


0 


2B3 


F33 


A33 


2E3 


D33 


M = 


0 


C 4 i 


0 


2B4 


F44 


A44 


2E4 


D44 




Cn 


2Bi 




All 


2Ei 


0 


Dn 


0 




C22 


2B2 


F22 


A22 


2E2 


0 


D22 


0 




D33 


2B3 


F33 


A33 


2E3 


0 


D33 


0 




IC44 


2B4 


F44 


A44 


2E4 


0 


D44 


0 



Now, since z is necessarily nonzero, eq.(8.62) should admit nontrivial 
solutions, and hence, matrix M should be singular, which leads to the 
condition below: 

det(M) = 0 (8.63) 

Thus, considering that all entries of M are linear in xi, det(M) is octic 
in xi, and hence, eq.(8.63) is equally octic in xi. By virtue of Fact 8.2, 
then, eq.(8.63) is of 16th degree in ti, i.e., it takes on the form 

16 

= 0 (8.64) 

0 

which is the characteristic equation sought, whose roots provide up to 16 
real values of 0\ for the IKP at hand. 
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8.2.4 Numerical Conditioning of the Solutions 

We recall here the concept of condition number of a square matrix (Golub 
and Van Loan, 1989), as introduced in Section 4.9. In this subsection we 
stress the relevance of the concept in connection with the accuracy of the 
computed solutions of the general IKP. 

The concept of condition number of a square matrix is of the utmost im- 
portance because it measures the roundoff-error amplification upon solving 
a system of linear equations having that matrix as coefficient. The con- 
dition number of a matrix, discussed in Section 4.9, can be computed in 
many possible ways. For the purpose at hand, it will prove convenient to 
work with the condition number defined in terms of the Probenius norm, 
as given in eqs.(4.126a-c). 

In the context of the bivariate-equation approach, we can intuitively 
argue that the accuracy in the computation of a solution is dictated by 
the angle at which the two contours giving a solution intersect. Thus, the 
solutions computed most accurately are those determined by contours in- 
tersecting at right angles. On the contrary, the solutions computed least 
accurately are those obtained by tangent contours. We shall formalize this 
observation in the discussion below. 

We distinguish between the condition number of a matrix and the condi- 
tioning of a solution of a nonlinear system of equations. We define the latter 
as the condition number of the Jacobian matrix of the system, evaluated 
at that particular solution. 

For concreteness, let 



/l(xi,X2) = 0 
/2(xi,X2) = 0 



be a system of two nonlinear equations in the two unknowns x\ and x^. 
Moreover, the Jacobian matrix of this system is defined as 



F = 



(V/2)^J 



(8.65) 



where V/s, denotes the gradient of function fk{xi, X 2 ), defined as 



V/fc 



dfk/dxi 

dfkjdx2 



( 8 . 66 ) 



It is to be noted that multiplying each of the two given equations by a 
scalar other than zero does not affect its solutions, each Jacobian column 
being, then, correspondingly multiplied by the same scaling factor. To ease 
matters, we will assume henceforth that each of the above equations has 
been properly scaled so as to render its gradient a unit vector in the plane 
of the two unknowns. In order to calculate the condition number of F, 
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which determines the conditioning of the solutions, we calculate first FF^ 
and its inverse, namely. 



FF^ 



1 


V/l-V/2" 




1 


COS 7 


.V/l-V/2 


1 




cos 7 


1 



and 



FF" 



sin^ 7 



1 

■ cos 7 



- cos 7 
1 



where 7 is the angle at which the contours intersect. The condition number 
K of F can then be computed as 



1 

I sin 7 1 



— 7T < 7 < 7T 



(8.67) 



which means that for the best possible solutions from the numerical condi- 
tioning viewpoint, the two contours cross each other at right angles, whereas 
at singular configurations, the contours are tangent to each other. The 
reader may have experienced that, when solving a system of two linear 
equations in two unknowns with the aid of drafting instruments, the solu- 
tion becomes fuzzier as the two lines representing those equations become 
closer and closer to parallel. 



8.2.5 Computation of the Remaining Joint Angles 

So far we have reduced the system of displacement equations to either 
a system of bivariate trigonometric equations in the sines and cosines of 
two joint angles — the bivariate-equation approach — or one single univariate 
polynomial in the tangent of half one of the joint angles — the univariate- 
polynomial approach. In either case, we still need a procedure to compute 
the remaining joint angles, which is the subject of the balance of this sub- 
section. 

The Bivariate-Equation Approach 

After all common intersections of the three foregoing contours have been 
determined, we have already two of the unknowns, 0 ^ and <? 5 , the remaining 
four unknowns being calculated uniquely as described presently. First, 
can be computed from eq.(8.33a), which can be rewritten in the form 

Hx3 = T ( 8 . 68 a) 

where the 6x2 matrix H and the 6 -dimensional vector r are both bilin- 
ear in X 4 and X 5 and hence, known. Although any two of the six equa- 
tions ( 8 . 68 a) suffice, in principle, to determine X3, we should not forget 
that these computations will most likely be performed with finite preci- 
sion, and hence, roundoff-error amplification is bound to occur. In order 
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to keep roundoff errors as low as possible, we recommend to use all the 
foregoing six equations and calculate X3 as the least-square approximation 
of the overdetermined system (8.68a). This approximation will be, in fact, 
the solution of the given system because all six equations are compatible. 
Moreover, this approximation can be expressed syrnholimlly in the form 

X3 = (8.68b) 

In practice, the foregoing least-square approximation is computed using an 
orthogonalization procedure (Golub and Van Loan, 1989), the explicit or 
the numerical inversion of the product H^H being advised against because 
of its frequent ill-conditioning. Appendix B outlines a robust numerical 
computation of the least-square approximation of an overdetermined sys- 
tem of equations using orthogonalization procedures. What is relevant to 
our discussion is that eq. (8.68b) determines 9^ uniquely for given values of 
and Or,. 

With <?3, and 9^, known, we can now calculate 9\ and simultaneously 
from eq.(8.26), which we reproduce below in a more suitable form 

Rxi 2 = X345 ( 8 . 69 ) 

where R is a 14 x 8 matrix depending only on the problem data, while 

X345, defined as 

X345 = PX45 ( 8 . 70 ) 

is a 14-dimensional vector trilinear in X3, X4, and X5, and hence, is known. 
Moreover, matrices P and R as well as vectors X12 and X45 were defined 
in eqs.(8.26) and (8.27a & b). Again, we have an overdetermined system, 
of 14 equations, in eight unknowns this time, which can best be solved 
for X12 using a least-square approach with an orthogonalization procedure. 
The unique solution of the overdetermined system at hand can thus be 
expressed as 

X12 = (R^R)^^R^X 345 ( 8 . 71 ) 

Note that the solution thus obtained determines xi and X2 uniquely, the 
only remaining unknown being 9q. This unknown is readily computed from 
eq.(4.9a). Indeed, the first of the three vector equations represented by this 
matrix equation yields 



Q1Q2Q3Q4Q5P6 = q ( 8 . 72 a) 

where q denotes the first column of Q, while, according to eq.( 8 . 5 ), pe 
denotes the first column of matrix Qe, i.e.. 



cos 9q 




qii 


sin 9q 


, q = 


921 


0 




_931 _ 



P6 = 



(8.72b) 
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Thus, eq. (8.72a) can be readily solved for pe, i.e., 

P6 = Q^QlQ3Q^Qrq (8-73) 

thereby obtaining a unique value for Oq for every set of values of }i, 
thus completing the solution of the IKP under study. 

The Raghavan and Roth Procedure 

The most straightforward means of computing 0^ and 0^ in this procedure is 
by means of eq.(8.51d), which can be interpreted as an eigenvalue problem 
associated with the 12x12 matrix S, and has one known eigenvalue, namely, 
0, for its sole variable, <? 3 , was computed so as to render it singular. Now, 
every scientific package offers eigenvalue calculations, whereby the eigen- 
vectors are usually produced in a normalized form, i.e., with all eigenvectors 
computed as unit vectors. Let, for example, a be the 12-dimensional eigen- 
vector of S corresponding to the zero eigenvalue. In this case, ||cr|| = 1, but 
X45, the solution sought, is obviously of magnitude greater than unity, for 
its 12th component, <ti2, is exactly 1, according to its definition, eq.(8.51e). 
In order to produce (X45) from cr, then, all we need is a suitable scaling 
of this vector that will yield (x 4 s)i 2 = 1. We thus have that <ti 2 0 — 

otherwise, eqs.(8.51d) would be inconsistent — and hence, 

1 

X45 = cr 

0'12 

The outcome will be a set of unique values of 0^ and 0^ for each of the 16 
possible values of <? 3 . 

Next, 0\ and O 2 are computed from eq.(8.39), which is rewritten below 
in a more suitable form: 

Rxi2 = X345 ( 8 . 74 a) 

with the the 14 -dimensional vector X345 defined as 

X345 = PX45 ( 8 . 74 b) 

Since R is a 14 x 8 matrix, eq.(8.74a) comprises 14 linear equations in the 
eight unknown components of X12. Again, a least-square approach to the 
solution of this system yields 

X 12 = (R^R)^^R^X345 (8.74c) 

the only remaining unknown being Oq, which is computed in exactly the 
same way as in the bivariate-equation approach. 

The Li, Woernle, and Hiller Procedure 

Once 0\ is available, the remaining angles are computed as indicated below: 
Equations (8.62) are first rearranged in nonhomogeneous form, i.e., as 

(8.75) 



Nz' = n 
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with the 8x7 matrix N and the 7- and 8-dimensional vectors z' and n 
defined as 





- 0 


Cn 


0 


2 Bi 


All 


An 


2Ar 




0 


C22 


0 


2B2 


A22 


A22 


2E2 




0 


C33 


0 


2B3 


A33 


A33 


2 A3 


N = 


0 


C44 


0 


2B4 


A44 


A44 


2A4 




Cii 


2 Bi 


All 


All 


2 Ei 


0 


All 




E22 


2B2 


F22 


A22 


2E2 


0 


A22 




E33 


2B3 


F33 


A33 


2 A3 


0 


A33 




IC44 


2B4 


F44 


A44 


2E4 


0 


A44 . 



and 



0 1 


[Alii 


A 7-3 
2 




A22 


A A 




A33 


A 




A44 


A A 
2 


, n = 


0 


A 




0 


A 




0 


- A - 




. 0 . 



Now, eq.(8.75) represents an overdetermined linear algebraic system of 
eight equations, but only seven unknowns. As usual, we recommend here a 
least-square approach to cope with ill-conditioning. In this way, the solution 
of eq.(8.75) can be expressed syrnholimlly in the form 

z' = (N^N)-^N^n 

With z' known, both T2 and T3, and hence, O 2 and 03, are known uniquely. 
Further, with 0i, 02, and 03 known, the right-hand side of eq. (8.58a), /3, 
is known. Since the coefficient matrix A of that equation is independent 
of the joint angles, A is known, and that equation can be solved for vec- 
tor X uniquely. Once x is known, the two angles 04 and 0s are uniquely 
determined, with 06 the sole remaining unknown; this can be readily deter- 
mined, also uniquely, as discussed in connection with the bivariate-equation 
approach. 

8.2.6 Examples 

We solve the examples below using the bivariate-equation approach with 
the purpose of helping the reader visualize the real solutions. To this end, 
we resort to eq. (8.33a) and thus, produce three determinants Aj(04,05), 
which, when equated to zero, produce three contours C, for i = 1,2,3. 
While we can choose these determinants in many possible ways, we do this 
by first partitioning the 6x3 matrix D of eq.(8.33a) into two 3x3 blocks, 
D„ being the upper, D; the lower block, which thus yields 

Ai = det(D„), A2 = det(Di) 
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Now, since the determinant is not additive, i.e., det(D„ + D;) yt det(D„) + 
det(D;), we choose A 3 as 

A 3 = det(D„ + D;) 

which is apparently independent of Ai and A 2 , thereby obtaining the three 
determinants, which, when equated to zero, yield the three contours sought. 

Example 8.2.1 In this first example, all inverse kinematic solutions of 
the Fanuc Arc Mate manipulator are found for the pose of the end-effector 
given below: 



'0 


1 


o' 




■ 130 ■ 


0 


0 


1 


, p = 


850 


1 


0 


0 




1540 



in which p is given in mm and the DH parameters of the manipulator are 
given in Table f.2. 

Solution: The solutions are obtained from the intersections of the three 
contours Ci, C 2 , and C 3 , as shown in Fig. 8.3. 

Four intersection points can be detected in this figure, which are num- 
bered 1, 2, 3, and 4. Moreover, at points 1 and 2 the three contours are 
tangent to one another. Tangency indicates the existence of a multiple root 
at that point, and hence, a singularity, as discussed in Subsection 4.5.2 in 
connection with decoupled manipulators. The numerical values of the joint 
angles of the four solutions are given in Table 8.2. 

Example 8.2.2 In this example, we discuss the IKP of DIESTRO, the 
isotropic six-axis orthogonal manipulator shown in Fig. f.31 (Williams et 
al, 1993). For a meaning of kinematic isotropy, we refer the reader to 
Section f.9. This manipulator has the DH parameters given in Table f.l. 

The pose of the end-effector leading to an isotropic posture, i.e., one 
whose .lacobian matrix is isotropic, is defined by the orthogonal matrix Q 
and the position vector p displayed below: 





'0 


-1 


0 ■ 




0 


Q = 


0 


0 


-1 


, p = 


-50 




1 


0 


0 




50 



with p given in mm. Compute all real inverse kinematics solutions at the 
given pose. 

TABLE 8.2. Inverse kinematics solutions of the Fanuc Arc Mate manipulator 



Sol’n 

No. 


9i 


62 


03 


04 


O5 


06 


1 & 2 


90° 


90° 


0° 


180° 


-180° 


0° 


3 


75.157° 


15.325° 


150.851° 


15.266° 


-103.353° 


176.393° 


4 


90° 


16.010° 


153.403° 


-180° 


100.588° 


0° 
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FIGURE 8.3. Contours Ci, C 2 , and C 3 for the Fanuc Arc Mate manipulator. 

Solution: The three contours in the 04-^5 plane, as discussed in Subsec- 
tion 8.2.2, are plotted in Fig. 8.4a. As this figure shows, the three contours 
intersect at two points, those labeled 1 and 2. The contours also intersect 
along a curve labelled SS in Fig. 8.4. 

In the same figure, the overlapping curve of the three contours represents 
a manifold of singular solutions, which means that this manipulator admits 
a set of self-motions, i.e., joint motions leaving the end-effector stationary. 
These motions can be explained by noticing that when the end-effector is 
located at the given pose and the manipulator is postured at joint- variable 
values determined by any point on the SS curve, the six links form a Bricard 
mechanism (Bricard, 1927). The degree of freedom of a Bricard mechanism 
cannot be determined from the well-known Chebyshev-Griibler-Kutzbach 
formula (Angeles, 1982), which yields a dof = 0. Here, the one-dof motion 
of the mechanism occurs because the six revolute axes can be laid out in 
such a way that if they are grouped in two alternating triads, then these 
triads intersect. The motion of all six links of the Bricard mechanism then 
is constrained to having a zero angular velocity about the line determined 
by the two intersection points. 

Furthermore, the contours Ci and C 2 intersect at right angles at solu- 
tion 1, which corresponds to the isotropic posture of the robot. The nu- 
merical values of the joint variables for the isolated solutions are given in 
Table 8.3. 




FIGURE 8 . 4 . Contours Ci, C2, and C3 for the DIEl 
pose: (a) full 64 ,- 6 ^ region; (b) a close-up of the aj 
at the point of coordinates 64 = 6^ = tt /2; and (c^ 
contour intersection southwest of solution 2 . 
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TABLE 8.3. Inverse kinematics solutions of the DIESTRO manipulator 



Solution No. 


0i 


O2 


O3 


9 i 


Ob 


Oe 


1 


0° 


90° 


-90° 


90° 


-90° 


180° 


2 


180° 


-90° 


90° 


-90° 


90° 


0° 



This example shows interesting features of the inverse kinematics of ma- 
nipulators which are not present in manipulators with simpler architec- 
tures, such as those with intersecting or parallel consecutive axes. Indeed, 
the point of coordinates 9 ^ = 0 ^ = tt/ 2 of Fig. 8.4a appears to be an in- 
tersection of the three contours, and hence, a solution of the IKP at hand. 
A close-up of this point, as displayed in Fig. 8.4b, shows that this point 
is indeed an intersection of all three contours, hut this point is, in fact, a 
double point, i.e., a point at which each contour crosses itself; this gives the 
point a special character: When verifying whether this point is a solution 
of the problem under study, we tried to solve for X 3 from eq.( 8 . 68 a), but 
then found that matrix H of that equation is rank-deficient, and hence, 
does not allow for the calculation of X 3 . An alternative approach to testing 
the foregoing values of 9 ^ and 9 ^ is described in Exercise 8.5. In following 
this approach, it was found that these values do not yield a solution, and 
hence, the intersection point is discarded. 

One more point that appears as an intersection of the three contours is 
that southwest of solution 2. A close-up of this point, as shown in Fig. 8.4c, 
reveals that the three contours do not intersect in that region. In summary, 
then, the manipulator at hand admits two isolated solutions at the given 
pose and an infinity of solutions along the curve SS. 

Example 8.2.3 Here we include an example of a manipulator admitting 
16 real inverse kinematics solutions. This manipulator was proposed by Li 
(1990), its Denavit-Hartenberg parameters appearing in Table 8.f. 

Solution: The foregoing procedure was applied to this manipulator for an 
end-effector pose given as 



■ -0.357279 


-0.850000 


0.387106' 




■ 0.798811 ■ 


0.915644 


-0.237000 


0.324694 


, P = 


-0.000331 


-0.184246 


0.470458 


0.862973 




1.200658 



TABLE 8.4. DH parameters of Li’s manipulator 



i 


ai (m) 


bi (m) 


Ck-i 


9i 


1 


0.12 


0 


-57° 


9i 


2 


1.76 


0.89 


35° 


O 2 


3 


0.07 


0.25 


95° 


9i 


4 


0.88 


-0.43 


79° 


9i 


5 


0.39 


0.5 


-75° 


9b 


6 


0.93 


-1.34 


-90° 


9e 
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where p is given in meters. 

The contours for this manipulator at the given EE pose are shown in 
Fig. 8.5, the numerical values of the 16 solutions being given in Table 8.5. 
Notice that the number of intersections in Fig. 8.5 appears to be 15, rather 
than 16. The missing intersection can be explained by the proximity of 
solutions 3 and 10, which appear in the same figure as a single intersection. 



8.3 Kinematics of Parallel Manipulators 

Unlike serial manipulators, their parallel counterparts are composed of 
kinematic chains with closed subchains. A very general parallel manipu- 
lator is shown in Fig. 8.6, in which one can distinguish two platforms, one 
fixed to the ground, B, and one capable of moving arbitrarily within its 
workspace, Ai. The moving platform is connected to the fixed platform 
through six legs, each being regarded as a serial manipulator, the leg thus 
constituting a six-axis serial manipulator whose base is B and whose end- 
effector is Ai. The whole leg is composed of six links coupled through six 
re volutes. 

The manipulator shown in Fig. 8.6 is, in fact, too general, and of little use 
as such. A photograph of a simpler and more practical manipulator of this 
kind, which is used as a flight simulator, is shown in Fig. f.5, its kinematic 
structure being depicted in Fig. 8.7a. In this figure, the fixed platform B is 
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TABLE 8.5. Joint angles of Li’s manipulator 



Sol’n 

No. 


0i 


92 


Os 


1 


174.083° 


-163.302° 


-164.791° 


2 


-159.859° 


-159.324° 


-111.347° 


3 


164.800° 


-154.290° 


-85.341° 


4 


-148.749° 


-179.740° 


-78.505° 


5 


-16.480° 


-10.747° 


-58.894° 


6 


-46.014° 


-19.256° 


-46.988° 


7 


-22.260° 


-22.431° 


-32.024° 


8 


-53.176° 


26.165° 


9.103° 


9 


-173.928° 


150.697° 


47.811° 


10 


-41.684° 


-29.130° 


52.360° 


11 


-137.195° 


-156.920° 


68.306° 


12 


-139.059° 


128.112° 


96.052° 


13 


-22.696° 


29.214° 


98.631° 


14 


-83.094° 


57.022° 


130.976° 


15 


1.227° 


-7.353° 


142.697° 


16 


177.538° 


-148.178° 


159.429° 



Sol’n 

No. 


O 4 


O 5 


Oe 


1 


-107.818° 


-155.738° 


141.281° 


2 


120.250° 


176.596° 


21.654° 


3 


4.779° 


-127.809° 


-101.359° 


4 


158.091° 


148.266° 


55.719° 


5 


-4.164° 


164.079° 


5.677° 


6 


-120.218° 


-145.864° 


-114.768° 


7 


-32.411° 


-172.616° 


-17.155° 


8 


145.868° 


136.351° 


127.978° 


9 


-21.000° 


-40.438° 


-92.284° 


10 


6.559° 


-129.124° 


25.091° 


11 


135.685° 


-51.347° 


147.446° 


12 


25.440° 


-7.345° 


-119.837° 


13 


-176.071° 


11.573° 


170.303° 


14 


67.570° 


-10.827° 


-110.981° 


15 


-123.878° 


-29.214° 


149.208° 


16 


-148.647° 


-129.278° 


110.984° 
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FIGURE 8.6. A general six-dof parallel manipulator. 

a regular hexagon, while the moving platform A4 is an equilateral triangle, 
as depicted in Fig. 8.7b. Moreover, B is connected to A4 by five revolutes 
and one prismatic pair. Three of the revolutes constitute a spherical joint, 
similar to the wrists studied in Section 4.4, while two more constitute a 
universal joint, i.e., the concatenation of two revolutes with intersecting 
axes. Of the foregoing six joints, only one, the prismatic pair, is actuated. 

It is to be noted that although each leg of the manipulator of Fig. 8.7a 
has a spherical joint at only one end and a universal joint at the other 
end, we represent each leg in that figure with a spherical joint at each 
end. Kinematically, the leg depicted in Fig. 8.7a is equivalent to the actual 
one, the only difference being that the former appears to have a redundant 
joint. We use the model of Fig. 8.7a only to make the drawing simpler. A 
more accurate display of the leg architecture of this manipulator appears 
in Fig. 8.8. 

Because the kinematics and statics of parallel manipulators at large are 
beyond the scope of this book, we will limit the discussion to parallel ma- 
nipulators of the simplest type. 

With regard to the manipulators under study, we can also distinguish be- 
tween the inverse and the direct kinematics problems in exactly the same 
way as these problems were defined for serial manipulators. The inverse 
kinematics of the general manipulator of Fig. 8.6 is identical to that of the 
general serial manipulator studied in Section 8.2. In fact, each leg can be 
studied separately for this purpose, the problem thus becoming the same 
as in that section. For the particular architecture of the manipulator of 
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FIGURE 8.7. A six-dof flight simulator: (a) general layout; (b) geometry of its 
two platforms. 

Fig. 8.7a, in which the actuated joint variables are displacements measured 
along the leg axes, the inverse kinematics simplifies substantially and al- 
lows for a simple closed-form solution. However, the direct kinematics of the 
same manipulator is as challenging as that of the general serial manipulator 
of Section 8.2. With regard to the direct kinematics of manipulators of the 
type depicted in Fig. 8.7a, Charentus and Renaud (1989) and Nanua et al. 
(1990) showed independently that like the inverse kinematics of general six- 
axis serial manipulators, the direct kinematics of this manipulator reduces 
to a 16th-degree polynomial. Note, however, that the direct kinematics of a 
manipulator similar to that of Fig. 8.7a, but with arbitrary locations of the 
attachment points of each leg to the moving and fixed platforms, termed 
the general platform manipulator, has been the subject of intensive research 
(Merlet, 2000). A breakthrough in the solution of the direct kinematics 
of platform manipulators of the general type was reported by Raghavan 
(1993), who resorted to polynomial continuation, a technique already men- 
tioned in Section 8.2, for computing up to 40 poses of A4 for given leg 
lengths of a parallel manipulator with legs of the type depicted in Fig. 8.8, 
but with attachment points at both A4 and B with an arbitrary layout. 
What Raghavan did not derive is the characteristic 40th-degree polyno- 
mial of the general platform manipulator. Independently, Wampler (1996) 
and Husty (1996) devised procedures to derive this polynomial, although 
Wampler did not pursue the univariate polynomial approach and preferred 
to cast the problem in a form suitable for its solution by means of polyno- 
mial continuation. Husty did derive the 40th-degree polynomial for several 
examples. In the process, he showed that this polynomial is the underlying 



326 



8. Kinematics of Complex Robotic Mechanical Systems 




FIGURE 8.8. A layout of a leg of the manipulator of Fig. 8.7 

characteristic polynomial for all manipulators of the platform type, which 
simplifies to a lower-degree polynomial for simpler architectures. As a mat- 
ter of fact, Lee and Roth (1993) solved the direct kinematics of platform 
manipulators for which the attachment points at the base and the moving 
platforms are located at the vertices of planar, similar hexagons. These re- 
searchers showed that the problem here reduces to a cascade of quadratic 
and linear equations. In the particular case in which both polygons are reg- 
ular, however, the manipulator degenerates into a movable structure, upon 
fixing the leg lengths, and hence, the solution set becomes a continuum, 
hazard and Merlet (1994), in turn, showed that the platform manipulator 
originally proposed by Stewart (1965), and known as the Stewart platform, 
has a 12th-degree characteristic polynomial. Interestingly, these mechani- 
cal systems were first introduced by Gough (1956-1957) for testing tires; 
Stewart (1965) suggested their use as flight simulators, an application that 
is now well established. 

Husty, however, did not show that his 40th-degree pol}momial is minimal 
in that manipulator architectures are possible that exhibit up to 40 actual 
solutions. Dietmaier (1998) did this, by devising an algorithm that would 
iteratively increase the number of real solutions of a given architecture. 
With this paper, Dietmaier proved conclusively that Husty’s 40th-degree 
polynomial is, in fact, minimal. This was rather surprising, for virtually 
everybody working in the field expected a minimal polynomial of a degree 
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TABLE 8.6. DH Parameters of the leg of Fig. 8.8 



i 


OjI 


hi 




1 


0 


0 


90 ° 


2 


0 


0 


90 ° 


3 


0 


bs 


0° 


4 


0 


64 (const) 


90 ° 


5 


0 


0 


90 ° 


6 


0 


&6 (const) 


0° 



of the form 2 ", with n being a positive integer. Notice that, in the cases of 
the most general serial six-revolute manipulator and of the flight simulator, 
the minimal polynomial is of a degree of this form, with n = 4 . 

Below we analyze the inverse kinematics of one leg of the manipulator of 
Fig. 8 . 7 a, as depicted in Fig. 8.8. The Denavit-Flartenberg parameters of 
the leg shown in this figure are given in Table 8.6. It is apparent that the 
leg under study is a decoupled manipulator. Its inverse kinematics can be 
derived by properly modifying the scheme introduced in Section 4 . 4 , for we 
now have a prismatic joint, which is, in fact, the only active joint of this 
manipulator. Moreover, by virtue of the underlying design, the active joint 
variable, 63, can take on only positive values. 

In view of the DH parameters of this manipulator, eq.( 4 . 16 ) reduces to 

QiQ2(a3 +a4) = c ( 8 . 76 ) 

where c denotes the position vector of the center C of the spherical wrist 
and, since frames JT3 and JF4 of the DH notation are related by a pure 
transformation, Q3 = 1 . Upon equating the squares of the Euclidean norms 
of both sides of the foregoing equation, we obtain 

I|a3 + a4f = ||cf ( 8 . 77 ) 

where by virtue of the DH parameters of Table 8.6, 

||a3 +a4p = (63 + hif 

Now, since both 63 and 64 are positive by construction, eq.( 8 . 77 ) readily 
leads to the desired inverse kinematics solution, namely, 

63= ||c|| -64 >0 ( 8 . 78 ) 

a result that could have been derived by inspection of Fig. 8.8. 

Note that the remaining five joint variables of the leg under study are 
not needed for purposes of inverse kinematics, and hence, their calculation 
could be skipped. However, in stud} 4 ng the differential kinematics of these 
manipulators, these variables will be needed, and so it is convenient to solve 
for them now. This is straightforward, as shown below: Upon expansion of 
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eq.(8.76), we derive three scalar equations in two unknowns, 0\ and 62 , 
namely, 

(63 + 64)52 = a:cci + ycsi (8.79a) 

-(&3 + &4)c2 = - 2c (8.79b) 

0 = xcsi-ycci (8.79c) 



in which Cj and Sj stand for cos Oi and sin 9i, respectively, while 63, occurring 
in the above equations, is available in eq.(8.78). Prom eq.(8.79c), 0\ is 
derived as 

61 = tan^^ (8.80a) 

which yields a unique value of 0 \ rather than the two lying tt radians apart, 
for the two coordinates xc and yc determine the quadrant in which 0 \ lies. 
Once 0\ is known, O 2 is derived uniquely from the remaining two equations 
through its cosine and sine functions, i.e.. 



zc _ xcci + ycsi 

bs + ^4 ^3 + ^4 



(8.80b) 



With the first three joint variables of this leg known, the remaining ones, 
i.e., those of the “wrist,” are calculated as described in Subsection 4.4.2. 
Therefore, the inverse kinematics of each leg admits two solutions, one for 
the first three variables and two for the last three. Moreover, since the only 
actuated joint is one of the first three, which of the two wrist solutions 
is chosen does not affect the value of 63, and hence, each manipulator leg 
admits only one inverse kinematics solution. 

While the inverse kinematics of this leg is quite straightforward, its direct 
kinematics is not. Below we give an outline of the solution procedure for the 
manipulator under study that follows the procedure proposed by Nanua et 
al. (1990). 

In Fig. 8.7a, consider the triangles MjS'jSj, for i = 1,2,3, where the 
subscript i stands for the *th pair of legs. When the lengths of the six legs 
are fixed and plate A4 is removed, triangle MjS'jSj can only rotate about 
the axis MjSj. Therefore, we can replace the pair of legs of lengths qia and 
qib by a single leg of length /j, connected to the base plate B by a revolute 
joint with its axis along MjSj. The resulting simplified structure, as shown 
in Fig. 8.9, is kinematically equivalent to the original structure in Fig. 8.7a. 

Now we introduce the coordinate frame with origin at the attachment 
point Oi of the *th leg with the base plate B, according to the convention 
below: 

For * = 1,2, 3, 



Oi is set at the center of the revolute joint; 

Xi is directed from Ai to Bi ; 

Yi is chosen such that Zi is perpendicular to the plane of the hexagonal 
base and points upwards. 
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FIGURE 8.9. Equivalent simplified mechanism. 

Next, we locate the three vertices S\, S 2 , and S '3 of the triangular plate 
with position vectors stemming from the center O of the hexagon. Fur- 
thermore, we need to determine li and Oi. Referring to Figs. 8.9 and 8.10, 
and letting a* and bj denote the position vectors of points Ai and Bj, 
respectively, we have 



di = ||bj - aj|| 




for * = 1, 2, 3, and hence, Uj is the unit vector directed from Ai to Bj. 
Moreover, the position of the origin Oi is given by vector Oj, as indicated 
below: 

Oi=ai+rjUi, for * = 1 , 2 , 3. (8.81) 

Furthermore, let Sj be the position vector of Si in frame (O*, Xj, 1), Zi). 
Then 

0 

Sj = -li cos (pi , 
k sin (pi 



for i = 1,2,3 



(8.82) 
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FIGURE 8.10. Replacing each pair of legs with a single leg. 



Now a frame IFq (O, X, Y, Z) is defined with origin at O and axes X and 
Y in the plane of the base hexagon, and related to Xi and Yi as depicted 
in Fig. 8.11. When expressed in frame Xq, Sj takes on the form 

[sj]o = [Oj]o + [RiJoSi, for * = 1,2, 3 (8.83) 

where [R*]o is the matrix that rotates frame Xq to frame JF*, expressed in 
JFq, and is given as 



cos Q!j — sin Q!j 0 
[ Rj ] 0 = sin a j cos a j 0 

[ 0 0 1 



for i = 1,2,3 



(8.84) 



Referring to Fig. 8.11, 



cos 0.i = U,' • 1 = Ui 



Sm Q!j = Uj • J = Mj. 



After substitution of eqs.(8.84)-(8.86) into eq.(8.83), we obtain 



Uiy COS (pi 

[ S* ] 0 [ ] 0 A cos (pi , 

sin (pi 



for i = 1,2,3 



where Oj is given by eq.(8.81). 

Since the distances between the three vertices of the triangular plate are 
fixed, the position vectors si, S2, and S3 must satisfy the constraints below: 

||s2-si|p = ai (8.88a) 

I|s3-S2p = a 2 ( 8 . 88 b) 

I|si-S 3 p = a 3 ( 8 . 88 c) 
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FIGURE 8.11. Relation between frames JFq and Ti. 

After expansion, eqs.(8.88a-c) take the forms: 

D\c4>\ + D2c4>2 + D2,c4>\c 4>2 + D4S(f)-iS(f)2 +£>5=0 (8.89a) 

EiC(p2 + E2C(p3 + E3C(p2c4>3 + Eis4)2s4>3 + £ 5=0 (8.89b) 

Eic4>i + E2c4>3 + E3c4>ic4>3 + Eis4>is4>3 + £ 5=0 (8.89c) 

where c(-) and s(-) stand for cos(-) and sin(-), respectively, while {£>j, Aj, Ei}\ 
are functions of the data only and bear the forms shown below*^: 

Di = 2/1(02 - oi)^Eui 
D2 = -2/2(02 - Oi)^Eu 2 

£>3 = -2/i/2U^Ui 

£>4 = —2/1/2 

£>5 = II02IP + ||oi|P — 2 o \ o 2 + /i + /| — tti 

El = 2/2(03 — 02)^Eu2 
£2 = —2/3(03 — 02)^Eu3 
£3 = — 2/2/3U3'u2 

£4 = —2/2/3 

£5 = ||c> 3 ||^ + ||c> 2 ||^ — 2O3O2 +12+13—0,2 



^Since all vectors in the 15 coefficients of interest are complanar, they are 
regarded as two-dimensional vectors. 
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Fi = 2/i(oi - 03 )^Eui 

F2 = —2/3(01 — 03)^Eu3 

E 3 = - 2 / 1/3 U 3 U 1 

Fi = — 2 / 1/3 

Pb = lloall^ + ||oi||^ ~ 2o|’oi + /i + /| — ttg 

In the above relations the 2x2 matrix E is defined as in eq.(4.100), and 
the frame in which the vectors are expressed is immaterial, as long as all 
vectors appearing in the same scalar product are expressed in the same 
frame. Since expressions for these vectors in Fq have already been derived, 
it is just simpler to perform those computations in this frame. 

Our next step is to reduce the foregoing system of three equations in 
three unknowns to two equations in two unknowns, and hence, obtain two 
contours in the plane of two of the three unknowns, the desired solutions 
being determined as the intersections of the two contours. Since eq. (8.89a) is 
already free of all we have to do is eliminate from equations (8.89b) 
and (8.89c). To do this, we resort to the usual trigonometric identities 
relating c<f>^ and with tan(<))3/2), in eqs. (8.89b) and (8.89c). After we 
have cleared the denominators by multiplying the two foregoing equations 
by (1 + t|) , the equations thus resulting take on the forms 

/cit| + /c 2 T 3 + /cg = 0 (8.90a) 

mirl + mgTg + mg = 0 (8.90b) 

where k\, /cg, and /cg are linear combinations of s(j) 2 , c 4 > 2 , and 1. Likewise, 
mi, mg, and mg are linear combinations of spi, cpi, and 1, namely, 

ki = Eic4>2 - E 2 - E3C(p2 + Ez 
k 2 = 2 EiS(p 2 

ks = Eic4>2 + E 2 + E3C(p2 + Ez 
nil = Ficpi - F 2 - E^cpi + F 5 
mg = 2 Fiis<pi 

mg = Ficpi + F 2 + E^cpi + F 5 

Next, we eliminate Tg from the above equations dialytically, as we did in 
Subsection 4.5.3 to find the workspace of a three-axis serial manipulator. 
We proceed now by multiplying each of the above equations by Tg to obtain 
two more equations, namely. 



/cit| + /c2t| + /cgTg = 0 (8.90c) 

miTg + mgT^ + mgTg = 0 (8.90d) 

Further, we write eqs.(8.90a)-(8.90d) in homogeneous form, i.e., as 

$Tg = 0 (8.91a) 
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with the 4x4 matrix $ and the 4-dimensional vector T 3 defined as 



ki 


k 2 


ks 


0 




\rl 1 


mi 


m2 


m3 


0 






0 


ki 


k 2 


ks 


, T3 = 


T3 


0 


mi 


m2 


m3 




1 



(8.91b) 



Equation (8.91a) constitutes a linear homogeneous system. Moreover, in 
view of the form of vector T 3 , we are interested only in nontrivial solutions, 
which exist only if det($) vanishes. We thus have the condition 



det($) = 0 (8.91c) 

Equations (8.89a) and (8.91c) form a system of two equations in two 
unknowns, (pi and (f> 2 - These two equations can be further reduced to a 
single 16th-degree polynomial equation (Nanua et ah, 1990), as discussed 
later on. 

In the spirit of the contour method introduced earlier, we plot these 
two equations as two contours in the <f>i-(f >2 plane and determine the de- 
sired solutions at points where the two contours intersect. Once a pair of 
{4>i, (P 2 ) values is found, (p^ can be uniquely determined from eqs. (8.89b & 
c). Indeed, these equations can be arranged in the form: 



EiS(p2 


E2 + E^C(p2 


S(p 3 




-EiC(p2 - E5 


FiS(pi 


E2 + EsC(pi 


_ C(p 3 




-Eic(pi - E5 



Prom the above equation, both cip^, and s(p 2 , can be found uniquely, with 
the foregoing unique values, <p^ is determined uniquely as well. 

Knowing the angles (pi, (p 2 , and (p^ allows us to determine the position 
vectors of the three vertices of the mobile plate, si, S2, and S3, whose 
expressions are given by eq.(8.87). Since three points define a plane, the 
pose of the end-effector is uniquely determined by the positions of its three 
vertices. We illustrate the foregoing procedure with a numerical example 
below: 

Example 8.3.1 (A Contour-Intersection Approach) Nanua et al. 
(1990) studied the direct kinematics of a manipulator of the kind under- 
analysis. This is a platform manipulator --whose base plate has six vertices 
with coordinates expressed with respect to the fixed reference frame JFq 
given below, with all data given in meters: 

Al = (-2.9, -0.9), Bi = (-1.2, 3.0) 

A 2 = ( 2.5, 4.1), B2 = { 3.2, 1.0) 

A 3 = ( 1.3, -2.3), 53 = (-1.2, -3.7) 

The dimensions of the movable triangular plate are, in turn. 



ai = 2 . 0 , 



02 — 2 . 0 , 



03 = 3.0 
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Determine all possible poses of the moving plate for the dimensions of the 
six legs given below: 



qia = 5.0, rjib = 4.5 

Q2a = 5.5, q2b = 5.0 

qsa = 5.7, q3b = 5.5 



Solution: After substitution of the given numerical values, eqs. (8.89a) and 
(8.91c) become, with Cj and Sj standing for cosfi and smfi, respectively, 

61.848 - 36.9561C1 - 47.2376c2 + 33.603ciC2 - 41.6822siS2 = 0 
-28.5721 + 48.6506C1 - 20.7097c? + 68.7942c2 - 100.811ciC2 
+ 35 . 9634 c?C 2 - 41.4096c? + 50.8539cic? - 15.613c?c? - 52.9789s? 

+ 67 . 6522 c 2 S? - 13.2765c?s? + 74.1623siS2 - 25.6617cisiS2 
-67.953c2SiS2 + 33.9241ciC2SiS2 - 13.202s? 

-3.75189cis? + 6.13542c?s? = 0 

The foregoing equations determine contours Ci and C 2 in the 4>i-(f>2 plane, 
which are plotted in Figs. 8.12. Four real solutions are found by superim- 
posing Cl and C2, as shown in the above figure. The numerical values of the 
solutions, listed in Table 8.7, agree with the published results. Solutions 
1 and 2 represent two poses of the triangular plate over the base, while 
solutions 3 and 4 are just the reflections of solutions 1 and 2 with respect 
to the plane of the base plate. Flence, the geometric symmetry gives rise to 
an algebraic symmetry of the solutions. 

Example 8.3.2 (The Univariate Polynomial Approach) Reduce the 
two equations found in Example 8.3.1, eqs. (8.89a) and (8.91c), to a .single 
monovariate polynomial equation. 

Solution: We first substitute the trigonometric identities relating cfy and 
sfi with Tj = tan(())j/2), for * = 1, 2, into eqs. (8. 89a) and (8.91c). Upon 
clearing the denominators by multiplying those equations by ( 1 +t? ) ( 1 +t| ) , 
we obtain two polynomial equations in namely, 

Git) + G2xf + G3T? + G4T1 + G5 = 0 
Hit) + H2T1 + F3 = 0 



TABLE 8.7. Solutions for Nanua et al.’s Example 



No. 


4>i (rad) 


4>2 (rad) 


4 > 2 , (rad) 


1 


0.8335 


0.5399 


0.8528 


2 


1.5344 


0.5107 


0.2712 


3 


-0.8335 


-0.5399 


-0.8528 


4 


-1.5344 


-0.5107 


-0.2712 



(8.92) 

(8.93) 
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4>2 (rad) 




4 >i (rad) 



FIGURE 8.12. Contours Ci and C 2 for Nanua et al.’s example. 



where 

Gi = + K2T^ + ifg 

G 2 = if4r| + K^T2 
G3 = + Krrl + Ks 

G4 = KqT2 + K1QT2 
Gs = ifiirl + ifi2r| + if 13 

and 

Hi = L1T2 + L2 
H 2 = HjT2 
Hs = L 4 T 2 + is 

In the above relations, {ifj}}® and {ij}i are all functions of the data. We 
now eliminate ti from eqs.(8.92) and (8.93), following Bezout’s method, 
as given in (Salmon, 1964). To do this, we multiply eq.(8.92) by Hi and 
eq.(8.93) by GiTi and subtract the two equations thus resulting, which 
leads to a cubic equation in ti, namely. 



(G 2 Hi - Giii 2 )rf + (GsHi - GiH 3 )t^ + G^Hin + G 5 H 1 = 0 (8.94a) 
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Likewise, if eq.(8.92) is multiplied by H\ti + H 2 and eq.(8.93) by Girf + 
G2T1 and the equations thus resulting are subtracted from each other, one 
more cubic equation in t\ is obtained, namely, 

(G1F3 - + (G4F1 + G 3 H 2 - G 2 H^)tI 

+{G^Hi + GiH 2 )Ti + G 5 H 2 = 0 (8.94b) 

Moreover, if we multiply eq.(8.93) by ti, a third cubic equation in ti can 
be derived, i.e., 

HitI + H 2 TI + F3T1 = 0 (8.94c) 

Now, eqs.(8.93) and (8.94a-c) constitute a homogeneous linear system of 
four equations in the first four powers of ti , which can be cast in the form 

Hti = 0 (8.95) 

where ti = [rf t\ t\ 1]^ and 

VG 2 H 1 -G 1 H 2 G^Hi-GiH^ GiHi G 5 H 1 I 

G3F1-G1F3 G3H2-G2H3 + GiHi GiH 2 + G^Hi G5F2 

Hi H 2 H 3 0 

0 Hi H 2 Hu . 

In order for eq.(8.95) to admit a nontrivial solution, the determinant of its 
coefficient matrix must vanish, i.e., 

det(H) = 0 (8.96) 

Given the definitions of {Gk}i and {Hk}i, it is apparent that Gi, G3, and 
G5 are quartic, while G 2 and G4 are cubic polynomials in T2. Likewise, 
Hi and H^ are quadratic, while H 2 is linear in T2 as well. As a result, the 
highest-degree entries of the first and second rows of H are sextic, while 
those of its third and fourth rows are quadratic. The outcome is that det(H) 
is of degree 6 + 6 + 2 + 2= 16, i.e., det(H) is a 16th-degree polynomial 
in T 2 - This equation, in general, admits up to 16 different solutions. Fur- 
thermore, the roots of the polynomial appear either in the form of complex 
conjugate pairs or real pairs. In the latter case, each pair represents two 
symmetric positions of the mobile platform with respect to the base, i.e., 
for each solution found above the base, another, mirror-imaged, solution 
exists below it. This symmetry exists, in general, as long as the six base 
attachment points are coplanar. 

Other parallel manipulators are the planar and spherical counterparts of 
that studied above, and sketched in Figs. 8.13 and 8.14. The direct kine- 
matics of the manipulator of Fig. 8.13 was found to admit up to six real 
solutions (Gosselin et ah, 1992), while the spherical manipulator of Fig. 8.14 
has been found to admit up to eight direct kinematic solutions (Gosselin 
et ah, 1994a, b). A comprehensive account of the simulation and design of 
three-dof spherical parallel manipulators, which includes workspace analy- 
sis as well, is included in (Gosselin et ah, 1995). 




8.3 Kinematics of Parallel Manipulators 337 



8.3.1 Velocity and Acceleration Analyses of Parallel 
Manipulators 

Now we proceed to the velocity analysis of the manipulator of Fig. 8.7a. 
The inverse velocity analysis of this manipulator consists in determining 
the six rates of the active joints, given the twist of the moving 

platform, t. The velocity analysis of a typical leg leads to a relation of the 
form of eq.(4.53), namely, 

Jj6j = tj, J = I, II, ■ ■ ■ , VI (8.97a) 



where Jj is the Jacobian of the Jth leg, Oj is the 6-dimensional joint- 
rate vector of the same leg, and tj is the twist of the moving platform 
AJ, with its operation point defined as the point Cj of concurrency of the 
three revolutes composing the spherical joint of attachment of the leg to 
the moving platform AJ, and shown in Fig. 8.8 as C, subscript J indicating 
that point C of that figure is different for different legs. We thus have 



Jj = 



Lj = 



ei 

63461 X 63 



62 

63462 X 63 



0 

63 



64 

0 



65 

0 



66 

0 



UJ 

Cj 



634 = 63 + 64 



(8.97b) 

(8.97c) 



where the leg geometry has been taken into account. 
Furthermore, from Fig. 8.8, it is apparent that 



cj = p - w X rj 



(8.98) 



with r J defined as the vector directed from Cj to the operation point P of 
the moving platform. 

Upon multiplication of both sides of the velocity relation of this leg, 
eq. (8.97a), by Ij from the left, with Ij suitably defined, we obtain a relation 
free of all unactuated joint rates. Indeed, a suitable definition of Ij is shown 
below: 

and hence, on the one hand. 



ifljOj = ( 63 )/ 

where the subscript J reminds us that 63 is different for each leg. In order 
to ease the notation, and since we have a single variable 63 per leg, we 
define henceforth 

6j = (63)/ (8.99a) 

and hence, the above relation between t j and the actuated joint rate of the 
Jth leg takes the form 



VjijOj = bj 



(8.99b) 




338 



8. Kinematics of Complex Robotic Mechanical Systems 



On the other hand, 

Ijtj = (eg )jCj 

Likewise, we define 



(e3)j = ej 

the foregoing relation thus }delding 



Ijtj = ejcj 



(8.100a) 

(8.100b) 



Note that vectors ej and r j define uniquely the line along the two attach- 
ment points of the Jth leg. Henceforth, this line will be termed the axis of 
the Jth leg. 

Upon equating the right-hand sides of eqs. (8.99b) and (8.100b), the de- 
sired expression for the actuated joint rate is derived, namely, 

6j = BjCj, J = VT (8.101a) 



That is, the Jth joint rate is nothing but the projection onto the Jth leg 
axis of the velocity of point Cj. Furthermore, upon substituting eq.(8.98) 
in eq.(8.101a) above, we obtain the relations between the actuated joint 
rates and the twist of the moving platform, namely. 



iij = [(ej X rj) 



P 






(8.101b) 



for J = I , II , . . . , VI. Upon assembling all six leg-equations of eq.(8.101b), 
we obtain the desired relation between the vector of actuated joint rates 
and the twist of the moving platform, namely. 



b = Kt 



(8.102a) 



with the 6-dimensional vectors b and t defined as the vector of joint vari- 
ables and the twist of the platform at the operation point, respectively. 
Moreover, the 6x6 matrix K is the Jacobian of the manipulator at hand. 
These quantities are displayed below: 



■ hi ■ 








- (e/ X riY 


T 

ej 


hn 


■, t = 


UJ 


, K = 


(e// X yh Y 


T 






_p_ 








-hvi- 








. {evi X rviY 


T 

ey/J 



(8.102b) 



From the above display, it is apparent that each row of K is the transpose 
of the Pliicker array of the corresponding leg axis, although in axis coordi- 
nates, as opposed to the Jacobian matrix J of serial manipulators, whose 
columns are the Pliicker coordinates of the corresponding joint axis in ray 
coordinates. Moreover, in these coordinates, the moment of the leg axis is 
taken with respect to the operation point P of AJ. One more difference 
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between the velocity analysis of serial and parallel manipulators is the role 
played by the actuator joint rates in the underlying forward and direct 
kinematics. In the case of parallel manipulators, this role is changed, for 
now we have that the actuator joint rates are given by explicit formulas in 
terms of the twist of the moving platform, along with the manipulator ar- 
chitecture and configuration. Finding the platform twist requires inverting 
matrix K. Moreover, the significance of singularities also changes: When K 
becomes singular, some instantaneous motions of the platform are possible 
even if all actuated joints are kept locked. That is, a singularity of K is 
to be interpreted now as the inability of the manipulator to withstand a 
certain static wrench. An extensive analysis of the singularities of parallel 
manipulators using line geometry in a form that is known as Grassrnann 
geometry was reported by Merlet (1989). 

Now, the acceleration analysis of the same leg is straightforward. Indeed, 
upon differentiation of both sides of eq.(8.102a) with respect to time, one 
obtains 





b = 


Kt + Kt 


(8.103a) 


where K takes the form 




• T 


• T ^ 

ef 






k = 


• T 


• T 

ej 


(8.103b) 






• T 


• T 




and uj is defined as 












Uj 


= ej X r J 


(8.103c) 


Therefore, 












Uj = 6j 


xrj + ejxrj (8.103d) 



Now, since vectors r j are fixed to the moving platform, their time-deriva- 
tives are simply given by 

rj=u!xrj (8.103e) 

On the other hand, vector ej is directed along the leg axis, and so, its 
time-derivative is given by 



ej = ujj X ej 

with ujj defined as the angular velocity of the third leg link, i.e., 

Wj = (0161 + 0262 )j 

the subscript J of the above parentheses reminding us that this angular ve- 
locity differs from leg to leg. Clearly, we need expressions for the rates of the 
first two joints of each leg. Below we derive the corresponding expressions. 
In order to simplify the notation, we start by defining 



fj = (ei)j, gj = (62 )j 



(8.103f) 
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Now we write the second vector equation of eq. (8.97a) using the foregoing 
definitions, which yields 



(^i)jfj X (6j + 64)0/ + (02)jgj X (6j + 64)0/ + hjQj = cj 



where 64 is the same for all legs, since all have identical architectures. Now 
we can eliminate ( 02 ) j from the foregoing equation by dot- multiplying its 
two sides by gj, thereby producing 



(^i)jgj X fj • (6j -h bi)e.j + +gJ(ejej)cj = g^cj 



where an obvious exchange of the cross and the dot in the above equation 
has taken place, and expression (8.101a) for bj has been recalled. Now it 
is a simple matter to solve for {9i)j from the above equation, which yields 



(0i)j 



gj(l -ejeJ)cj 
Aj 



with Aj defined as 

Aj = (6j -h &4)ej X fj • gj ( 8 . 104 ) 

Moreover, we can obtain the above expression for {9i)j in terms of the 
platform twist by recalling eq.(8.98), which is reproduced below in a more 
suitable form for quick reference: 

cj = Cjt 

where t is the twist of the platform, the 3x6 matrix Cj being defined as 



Cj = [Rj 1] 

in which Rj is the cross-product matrix of rj and 1 is the 3x3 identity 
matrix. Therefore, the expression sought for {9i)j takes the form 

(^i)j = --^gj(l -eje5)Cjt, J = /,//,... ,K/ (8.105a) 



A similar procedure can be followed to find (<^2)/, the final result being 

(<?2)j = -^fJ(l-ejeJ)Cjt, J = /,//,...,!// (8.105b) 

thereby completing the calculations required to obtain the rates of all actu- 
ated joints. Note that the unit vectors involved in those calculations, ej, fj, 
and gj, are computed from the leg inverse kinematics, as discussed above. 
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Planar and Spherical Manipulators 

The velocity analysis of the planar and spherical parallel manipulators of 
Figs. 8.13 and 8.14 are outlined below: Using the results of Subsection 4.8.2, 
the velocity relations of the Jth leg of the planar manipulator take the form 

Jj0j = t, J = I, //, III (8.106) 

where Jj is the Jacobian matrix of this leg, as given by eq.(4.105), while 
6j is the 3-dimensional vector of joint rates of this leg, i.e.. 





' 1 1 1 ' 






Jj = 


Erji Erj2 Erj3_ 


, = 


1 

CO to 

1 



For purposes of kinematic velocity control, however, we are interested 
only in the first joint rate of each leg; i.e., all we need to determine in order 
to produce a desired twist of the end-effector is not all of the foregoing nine 
joint rates, but only 6ji, 9jji, and 9 mi. Thus, we want to eliminate from 
eq.(8.106) the unactuated joint rates 9j2 and 0 j 3 , which can be readily 
done if we multiply both sides of the said equation by a 3-dimensional 
vector rij perpendicular to the second and the third columns of Jj. This 
vector can be most easily determined as the cross product of those two 
columns, namely, as 



n = jj2 X jj 3 



-rj2Erj3 

r J2 - r J3 



Upon multiplication of both sides of eq.(8.106) by rij, we obtain 

[-rj2Erj3 + (rj2 - rj3)^Erji] 9ji = -(rj2Erj3)u; + (rj2 - rj3)^c 

(8.107) 

and hence, we can solve directly for 9j\ from the foregoing equation, thereby 
deriving 

^ ^ ~(rj2Erj3)t^ + (rj2 - rj3)^c 

-r52Erj3 + (rj2 - rj3)^Erji 

Note that eq.(8.107) can be written in the form 

jj9ji = kjt, J = /, //, III 
with jj and kj defined, for J = I, II, III, as 



(8.108a) 

(8.108b) 



jj = (rj2 - rj3)^Erji - rj2Erj3, 
kj = [rj2Erj3 (rj2 - 



6=[9n 9 h 2 9iii^^ 



If we further define 
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and assemble all three foregoing joint-rate-twist relations, we obtain 



J 6 > = Kt 



(8.109) 



where J and K are the two manipulator Jacobi ans defined as 



J = diag(j/, jn^ jm), K = 



kf 

kfr 

kfrr 



( 8 . 110 ) 



Expressions for the joint accelerations can be readily derived by differenti- 
ation of the foregoing expressions with respect to time. 

The velocity analysis of the spherical parallel manipulator of Fig. 8.14 
can be accomplished similarly. Thus, the velocity relations of the Jth leg 
take on the form 

Jj0j = w, J = /,//,/// (8.111) 

where the Jacobian of the Jth leg, Jj, is defined as 



Jj = [eji ej2 ej3 ] 



while the joint-rate vector of the Jth leg, 0j, is defined exactly as in the 
planar case analyzed above. Again, for kinematic velocity control purposes, 
we are interested only in the actuated joint rates, namely, 6ji, 9jji, and 
Ouii ■ As in the planar case, we can eliminate 0j2 and 0j3 upon multiplica- 
tion of both sides of eq.(8.111) by a vector rij perpendicular to the second 
and the third columns of Jj. An obvious definition of this vector is, then. 



nj = ej2 X ej3 

The desired joint-rate relation is thus readily derived as 

J = 1,11, III (8.112) 

where jj and kj are now defined as 

jj = eji X ej 2 • ej 3 (8.113a) 

kj=ej2Xej3 (8.113b) 

The accelerations of the actuated joints can be derived, again, by differen- 
tiation of the foregoing expressions. 

We can then say that in general, parallel manipulators, as opposed to 
serial ones, have two Jacobian matrices. 



8.4 Multifingered Hands 

Shown in Fig. 8.15 is a three-fingered hand with fingers A, B, and C, each 
supplied with three revolute joints. Furthermore, each finger carries two 
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revolutes of parallel axes that are normal to the axis of the third one. Thus, 
each finger comprises three links, the one closest to the palm V being of 
virtually zero length and coupled to V via a re volute joint. Of the other 
two, that in contact with the object O is the distal phalanx, the other being 
the proximal phalanx. Moreover, the fingers can be either hard or soft; if 
the latter, then contact takes place over a finite area; if the former, then 
contact takes place over a point, and hence, hard fingers can exert only 
force and no moment on the manipulated object. Soft fingers can exert 
both force and moment. For the sake of conciseness, we will deal only with 
hard fingers here. Let the contact points of fingers A, B, and C with O 
be denoted by Aq, Bq, and Co, respectively. The purpose of the hand is 
to manipulate O with respect to V. The motion of O, moreover, can be 
specified through its pose, given in turn by the position vector o of one of 
its points, O, and its orientation matrix Q with respect to a frame fixed 
to V ■ Now, in order to manipulate O six degrees of freedom are needed. 
When the three fingers are in contact with O, the hand-object system forms 
a parallel manipulator with three “legs” of the RRS type, with S standing 
for spherical joint. As the reader can verify, the system has six-dof, which 
means that manipulations are possible with only two actuated revolutes 
per finger. Many designs involve only two motors per finger, one of the 
revolute joints being provided with springs to guarantee contact. 

Thus, the location of the three contact points is fully determined if the 
pose of V and the locations of Aq, Bq, and Co in O are given. Once 
the position vectors of the three contact points are known, determining 
the joint-variable values needed to take O to the desired pose reduces to 
solving a 3-dimensional positioning problem for each finger, with three 
revolute joints — a problem already discussed in Subsection 4.4.1. The joint 
rates and accelerations are then determined as in Sections 4.4 and 4.6. 





FIGURE 8.15. A three-fingered hand. 
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While the mechanics of grasping is quite elaborate, due to the defor- 
mation of both fingers and object, some assumptions will be introduced 
here to produce a simple model. One such assumption is rigidity; a second 
is smoothness, under which each finger is capable of exerting only normal 
force on the object. Moreover, this force is unidirectional, for the finger can- 
not exert a pull on the object. The smoothness and rigidity assumptions 
bring about limitations, for they require a rather large number of fingers 
to exert an arbitrary wrench on the grasped object, as shown below. 

We assume that we have a rigid object O bounded by a surface S that is 
smooth almost everywhere, i.e., it has a well-defined normal n everywhere 
except at either isolated points or isolated curves on S. Below we show that 
in order to exert an arbitrary wrench w onto O, a hand with rigid, smooth 
fingers should have more than six fingers. Assume that the n contact points 
on S are {P*}" and that we want to find n pressure values {Aj}" at the 
contact points that will produce the desired wrench w onto O. 

Moreover, let the unit normal at Pi be denoted by rij and the vector 
directed from O to Pi be denoted by pj, as shown in Fig. 8.16. 

The wrench Wj exerted by each finger onto O at Pi is clearly 



Wj 



Ai 



Pi X (-rij) 

-rii 



Ai >0 



Upon equating the resultant wrench with the desired wrench, we obtain 



E 



-Pi X rii 

-rii 



Ai 



w 



or in compact form, as 

GA = — w (8.114a) 

where G is the 6 x n grasping matrix and A is the n-dimensional vector of 
pressure values, i.e.. 









' Ai " 


Pi X 111 • 


• • Pn X n„ 


, A = 




111 


n„ 












- - 



(8.114b) 




FIGURE 8.16. Geometry of grasped object O. 
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FIGURE 8.17. A prototype of the KU Leuven three-fingered hand (courtesy of 
Prof H. van Brussel). 

Note that the *th column of the grasping matrix is nothing but the array 
of Pliicker coordinates of the line of action of the force exerted by the *th 
finger on the object, in ray coordinates — see Subsection 3.2.2. 

Thus, for n = 6, a unique pressure vector A is obtained as long as G is 
nonsingular. However, negative values of {Aj}" are not allowed, and since 
nothing prevents these values from becoming negative, six fingers of the 
type considered here are not enough. We must thus have more than six 
such fingers in order to be able to apply an arbitrary wrench onto the 
body. For n > 6 and a full-rank 6 x n grasping matrix, nonnegative values 
of {Aj}" can be generated, but only under certain conditions, as explained 
below: Let u be a vector lying in the nullspace of G, i.e., such that Gu = 0. 
Then an arbitrary A can be expressed as 

A = Ao + u 

where Aq is a particular solution of eq.(8.114a). For example, if Aq is chosen 
as the minimum-norm solution of eq.(8.114a), then we have, explicitly, 

Ao = -G+w 

where Gt is the generalized inverse of G, defined as 

Gt = G^(GG^)- 
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The numerical computation of the minimum norm solution of an undeter- 
mined system of linear equations is discussed in Appendix B. 

Note that the 6x6 product GG^ has the general form 

PPT _ r El (Pi X rii)(Pi X rii)^ El (Pi X rii)nf 

El ni(Pi X rij)-' rijn/ 

Although a symbolic expression for the inverse H of GG^ is not possible 
in the general case, we can always express this inverse in block form, with 
all blocks of 3 X 3, namely, 

Hs(GGp-'= 2“ 

t±12 ■*^ 22 _ 

where consistently, Hu has units of meter^^, H 12 has units of meter^^, 
and H 22 is dimensionless. Moreover, we can partition G into two 3 x n 
blocks, i.e.. 




in which A has units of meter, while B is dimensionless. Hence, the product 
G^H takes on the form 

G^H= [A^Hii+B^Hf2 A^Hi2+B^H22] 

and hence, the left-hand block of the foregoing product has units of meter^^, 
while the right-hand block is dimensionless. Upon multiplying the desired 
wrench w from the left by this product, the result, Aq, has consistently 
units of Newton. 

Now, to find u, several numerical methods are available that do not 
require any matrix inversion (Golub and Van Loan, 1989). A simple way 
of expressing u, although by no means the way to compute it, is given by 

u = Pv, P = l-GtG 

where v is an n-dimensional vector and P is a matrix projecting v onto the 
nullspace of G, and 1 defined as the nxn identity matrix. Now we are left 
with the task of finding v so that 

Aq^ T % 1, ..., Tb 

Hence, our policy to determine u is simply, for * = 1, ..., n, 

^ ^ f 0, if Aoi > 0; 

® ( — Aoi, otherwise. 

Now, V is found upon solving 



Pv = u 
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However, P is singular — its rank of n — 6, as the reader is invited to prove — 
and the above equation may or may not admit a solution. For a solution 
to be possible, u must lie in the range of P. 

A more general approach to solving the grasping problem relies on linear 
programming, but this topic lies beyond the scope of the book. The inter- 
ested reader is directed to the specialized literature on the topic (Hillier 
and Lieberman, 1995). 

In the presence of soft fingers, however, fewer than six fingers suffice to 
grasp an object. Moreover, in the presence of friction, the force transmitted 
by a finger has, in addition to its normal component, a tangential compo- 
nent that, hence, gives rise to a contact force making a nonzero angle with 
the normal rij to the object surface at the *th contact point. Therefore, by 
virtue of the linear relation between the normal and the tangential com- 
ponents of the transmitted force, given by the coefficient of friction p,, this 
force is constrained to lie within the friction cone. This cone has its apex 
at the contact point Pi, its elements making an angle a with the normal, 
that is given by a = arctan(p). Furthermore, by virtue of the fundamental 
assumption of Coulomb friction analysis, p lies between 0 and 1, and hence, 
a is constrained to lie between 0° and 45°. 

Shown in Fig. 8.17 is an example of a three-fingered hand. This hand 
was developed at the Katholieke Universiteit Leuven (Van Brussel et al., 
1989). 

The literature on multifingered hands and the problem of grasping is 
far richer than we can afford to describe here. Extensive studies on these 
subjects have been recently reported by Reynaerts (1995) and Teichmann 
(1995.) 

8.5 Walking Machines 

Besides the walking machines introduced in Chapter 1, namely, the OSU 
Adaptive Suspension Vehicle and the TUM Hexapod, other legged ma- 
chines or leg designs are emerging with special features. For example, 
CARL, shown in Fig. 8.18, is a compliant articulated robot leg that has 
been designed at McGill University’s Centre for Intelligent Machines (CIM) 
by Prof. Buehler and his team (Menitto and Buehler, 1996). This leg con- 
tains an actuation package with a high load-carrying capacity (ATLAS) and 
an antagonistic pair of concentric translational-to-angular displacement de- 
vices. The leg has four degrees of freedom, of which two are actuated by 
ATLAS and one by a harmonic drive motor, while one is unactuated. This 
leg design is intended to provide locomotion to a quadrupled. 

As nature shows in mammals, four legs are necessary to guarantee the 
static equilibrium of the body while one leg is in the swing phase. Static 
equilibrium is achieved as long as the horizontal projection of the mass 
center of the overall body-legs system lies within the triangle defined by 
the contact points of the three legs that are in the stance phase. More than 
four legs would allow for greater mobility. For purposes of symmetry, some 
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FIGURE 8.18. The compliant articulated robot leg (courtesy of Prof. M. 
Buehler) . 

walking machines are designed as hexapods, so as to allow for an equal 
number of legs in the swing and the stance phases. 

We undertake the kinematic analysis of walking machines using the hexa- 
pod displayed in Fig. 8.19. 

Furthermore, contact with the ground is assumed to take place such that 
the ground can exert only a “pushing” force on each leg but no moment. 
Thus, while we can model the contact between leg and ground as a spherical 
joint, care must be taken so that no pulls of the ground on the leg are 
required for a given gait. 

Additionally, we shall assume that the leg is actuated by three revolutes, 
namely, those with variables <? 4 , <? 5 , and Oq in Fig. 8.20, where Q denotes 
the ground and B the machine body. A photograph of one of the six iden- 
tical legs of the walking machine developed at the Technical University 
of Munich, introduced in Fig. 1.9, is included in Fig. 8.21. The Denavit- 
Flartenberg parameters of this leg, proceeding from the ground upwards, 
are displayed in Table 8.8. Note that the architecture of this leg is sim- 
ply that of a three-revolute manipulator carrying a spherical joint at its 
end-effector, similar to that of the decoupled manipulators studied in Sec- 
tion 4.4. The spherical joint accounts for the coupling of the leg with the 
ground. We are thus assuming that when a leg is in contact with the ground, 
the contact point of the leg is immobile. At the same time, the motion of 
the body B is prescribed through the motion of a point on the axis of the 
revolute coupled to the body. Such a point is indicated by Pj for the Jth 
leg. Moreover, the point of the Jth leg in contact with the ground will be 
denoted by Oj. Thus, when prescribing the motion of the body through 
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FIGURE 8.19. A general hexapod. 



that of each of the six points Pi, Pu, . . Pyi, the rigid-body compatibility 
conditions of eqs.(7.14), (7.15), and (7.25) must be observed. The pose of 
the body B is thus specified by the position of a point C of the body and 
the orientation matrix Q of the body with respect to a frame fixed to the 
ground, the position vector of C in that frame being denoted by c. The 
specification of points Pj to Pyi thus follows from the knowledge of c and 
Q, thereby guaranteeing compliance with the above-mentioned constraints. 

Furthermore, a maneuver of B, given by a prescribed pose, can be achiev- 
ed by suitable values of the actuated-joint variables, which thus leads to a 
problem of parallel-manipulator inverse kinematics. 

The mechanical system that results from the kinematic coupling of the 
machine legs with the ground is thus equivalent to a parallel manipulator. 
The essential difference between a walking machine and a parallel manip- 
ulator is that the former usually involves more actuators than degrees of 
freedom. This feature is known as redundant actuation and will not be 



TABLE 8.8. DH Parameters of the leg of the TU-Munich walking machine 



i 


a,i (mm) 


6j (mm) 




1 


17 


0 


o 

o 


2 


123 


0 


180° 


3 


116 


0 


0° 


4 


0 


0 


O 

o 


5 


0 


0 


O 

o 


6 


0 


0 


0° 




le of the six identical legs of the TU M 
r. Reproduced with permission of TSl 
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pursued here. 



8.6 Rolling Robots 

Probably the rolling robot that has received most media attention is NASA’s 
Sojourner, of the Pathfinder mission, which explored a spot of the Martian 
landscape for several months in 1997. It is noteworthy that the Sojourner 
was designed, built, and commissioned with a shoestring budget for NASA 
standards. The Sojourner is a paradigm of rolling robots for autonomous 
operation on rough terrain. We focus here on the simplest robots of this 
class, i.e., robots intended for tasks on horizontal surfaces, and so, their 
platforms undergo planar motion, which greatly simplifies their kinemat- 
ics. One special feature of rolling robots is their nonholonomic nature. What 
this means is that the minimum number m of generalized coordinates defin- 
ing uniquely a posture of the system is greater than the number n of their 
independent generalized speeds, i.e., m > n. In the case of serial and paral- 
lel manipulators, paradigms of holonomic systems, m = n. In nonholonomic 
systems, then, we must distinguish between their posture, or configuration 
degree of freedom and their velocity degree of freedom. For the sake of 
conciseness, we will refer to the latter whenever we mention the degree of 
freedom of a rolling robot. 

Rolling robots are basically of two kinds, depending on whether they 
are supplied with conventional or with omnidirectional wheels. The sim- 
plest robots with conventional wheels are capable only of 2-dof motions, 
and hence, are kinematically equivalent to conventional terrestrial vehicles. 
However, robots with omnidirectional wheels (ODWs) are capable of 3- 
dof motions, which increases substantially their maneuverability. Below we 
outline the kinematics of the two kinds of robots. 

8.6.1 Robots vMh Conventional Wheels 

We begin with robots rolling on conventional wheels. Since these have two 
degrees of freedom, they need only two actuators, the various designs avail- 
able varying essentially in where these actuators are located. The basic 
architecture of this kind of robot is displayed in Fig. 8.22a, in which we 
distinguish a chassis, or robot body, depicted as a triangular plate in that 
figure: two coaxial wheels that are coupled to the chassis by means of revo- 
lutes of axes passing through points Oi and O 2 ] and a third wheel mounted 
on a bracket. 

Now, the two actuators can be placed in two essentially different arrays. 
In the first array, not shown in the figure, one actuator is used for propulsion 
and the other for steering, the former being used to provide locomotion 
power to the common two-wheel axle via a differential gear train. This 
train is required to allow for different angular velocities of the two coaxial 
wheels. Moreover, the orientation of the mid-plane of the steering wheel, 
defined by angle 'ip, is controlled with the second actuator. This design 
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FIGURE 8.22. A 2-dof rolling robot: (a) its general layout; and (b) a detail of its 
actuated wheels. 



has some drawbacks, namely, (*) the two motors serving two essentially 
different tasks call for essentially different operational characteristics, to 
the point that both may not be available from the same manufacturer; (m) 
the power motor calls for velocity control, the steering motor for position 
control, thereby giving rise to two independent control systems that may 
end up by operating in an uncoordinated fashion; and finally, (in) the use 
of a differential gear train increases costs, weight, and brings about the 
inherent backlash of gears. 

In the second actuation array, shown in Fig. 8.22b, the two coaxial wheels 
are powered independently, thereby doing away with the differential train 
and its undesirable side effects, the third wheel being an idle caster. More- 
over, the orientation of the latter is determined by friction and inertia 
forces, thereby making unnecessary the steering control system of the first 
array. Below we analyze the kinematics of a robot with this form of actua- 
tion. 

Let point C of the platform be the operation point, its position vector in 
a frame fixed to the ground being denoted by c. Additionally, let uj be the 
scalar angular velocity of the platform about a vertical axis. By virtue of 
the 2-dof motion of this robot, we can control either the velocity c of (U or 
a combination of uj and a scalar function of c by properly specifying the 
two joint rates 0\ and O 2 ■ However, we cannot control the two components 
of c and uj simultaneously. 

In order to proceed with the kinematic analysis of the system at hand, 
we define an orthonormal triad of vectors whose orientation is fixed with 
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respect to the chassis. Let this triad be denoted by { i, j, k }, with k point- 
ing in the upward vertical direction. Thus, the velocities 6j of points Oj, 
for * = 1, 2, are given by 



6j = r<ijj, *=1,2 (8.115a) 

and moreover, the angular velocity uj of line O 1 O 2 in planar motion, which 
is the same as that of the platform, can be readily expressed as 

^ = ^(01-02) (8.115b) 

its positive direction being that of k. 

Furthermore, the velocity of C can now be written in 2-dimensional form 
as 

c = 6j + ct;E(c' — Oj), * = 1,2 (8.115c) 

with c' denoting the position vector of point C , the orthogonal projec- 
tion of C onto the horizontal plane of Oi and O 2 , while E is as defined in 
eq.(4.100). Thus, all vectors of eq.(8.115c) are 2-dimensional. Upon substi- 
tution of eqs.(8.115a & b) into eq.(8.115c), we obtain expressions for c in 
terms of the joint rates, similar to eqs.(8.115c), for * = 1,2. Furthermore, 
upon adding sidewise the two expressions thus resulting, we obtain c in the 
desired form, namely. 



c = ay(0i - 02)i + ^(^1 + ^ 2 )j (8.115d) 

Equations (8.115b & d) express now the differential direct kinematics 
relations of the robot under study. In compact form, these relations become 

t = h9a (8.115e) 



with the 3x2 matrix L defined as 



L = 



r/l 

(ar//)i + (r/2)j 



—r/l 

(ar//)i + (r/2)j 



(8.1151) 



Moreover, the planar twist t of the platform and the 2-dimensional vector 
6a of actuated joint rates are defined as 



t = 




1 

'2 



(8.115g) 



Computing the joint rates from the foregoing equations, i.e., solving the 
associated inverse kinematics problem, is now a trivial task. The inverse 
kinematics relations are computed below by noticing that eq.(8.115b) pro- 
vides a relation for the joint-rate difference. Thus, all we need now is a 
second equation for the joint-rate sum. By inspection of eq.(8.115d), it is 
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apparent that we can derive this relation by dot-multiplying both sides of 
this equation by j, thereby obtaining 

c-j = ^(0i+02) (8.116) 

The two equations (8.115b) and (8.116) can now be cast into the usual 
form 

36a = Kt (8.117a) 

where the two robot Jacobians J and K are given below: 





0 ^ ■ 
(2/r)j^_ 



(8.117b) 



Note that J is a 2 x 2 matrix, but K is a 2 x 3 matrix. 

The inverse kinematics relations are readily derived from eq.(8.117a), 
namely, 



h 




where y = c • j . 

Now, in order to complete the kinematic analysis of the robot at hand, 
we calculate the rates of the unactuated joints, and 'tp. To this end, 
let tUj, for i = 1,2,3, and 63 denote the 3-dimensional angular velocity 
vector of the *th wheel and the 3-dimensional velocity vector of the center 
of the caster wheel. Likewise, UJ 4 denotes the scalar angular velocity of the 
bracket. 

We thus have, for the angular velocity vectors of the two actuated wheels, 

+ Lvk = — <iii + y (^1 “ ^2)k 

= [-i+(r//)k -(r//)k] (8.118a) 

[C2 _ 

W2 = -O21 + u;k = -021 + y(^i “ ^2)k 

= [(r//)k -i-(r//)k] (8.118b) 

In the ensuing derivations, we will need the velocities of the centers of 
the two actuated wheels, which were derived in eq.(8.115a). Moreover, the 
angular velocity of the caster wheel can be written most easily in the frame 
fixed to the bracket, {03, f3, k}, namely. 



= ^*363 + (t^ + V>)k 



( 8 . 119 ) 
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FIGURE 8.23. Layout of the unit vectors fixed to the platform and to the bracket. 

with ip denoting the angle between vectors j and 63 of Fig. 8.22a, measured 
in the positive direction of k, as indicated in the layout of Fig. 8.23. 

Note that vector 63 is parallel to the axis of rolling of the caster wheel, 
while f 3 is a horizontal vector perpendicular to 63 . These two sets of unit 
vectors are related by 



63 = — sin ipi + cos ipj 


( 8 . 120 a) 


f 3 = — cos ipi — sin ipj 


( 8 . 120 b) 


their inverse relations being 




i = — sin ipe^ — cos ipf^ 


( 8 . 120 c) 


j = cos ipe^ — sin ipf^ 


( 8 . 120 d) 



Furthermore, the velocity of the center of the caster wheel is derived as 

63 = W3 X rk = 



while the scalar angular velocity of the bracket, CJ4, is given by 

ui4 = u; + ip = j(0i - 02) + ip ( 8 . 121 ) 

In Chapter 10 we shall need c in bracket coordinates. Such an expression 
is obtained from eqs.(8.115d) and (8.120c & d), namely, 

c = - 62 ) sin + O 2 ) cos V’]e 3 

- 62 ) cosip+ ^{9i + ^ 2 ) sin V>]f 3 ( 8 . 122 ) 

Expressions for the dependent rates in terms of the independent ones, 
9 1 and 62 , are readily derived. To this end, we express the velocity of P in 
two independent forms, one in terms of the velocity of O 3 and the other in 
terms of the velocity of C, i.e., 

p = 63 + LV4k X (p - 03 ) 
p = c + ct;k X (-6j) 



(8.123a) 

(8.123b) 
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Upon equating the right-hand sides of the above equations, we obtain a 
3-dimensional vector equation relating dependent with independent rates, 
namely, 

-r^afa + (u; + ^/>)k x (p - Oa) = c + bivi 

where we have recalled the expressions derived above for 6a and [J 4 . Further, 
we rewrite the foregoing equation with the unknown rates, <?a and 'ip, on 
the left-hand side, i.e., 

— rliafa + V’k x (p — Oa) = c + bivi — uj\i x (p — Oa) (8.124) 

Moreover, we note that, from Fig. 8.22, 

p - Oa = -dfa + {h- r)k 



and hence. 



k X (p - Oa) = dea 



equation (8.124) thus becoming 



-rdafa + tpde^ = c + u;{bi - doa) 



(8.125) 



Now it is a simple matter to solve for da and tp from eq.(8.125). Indeed, 
we solve for da by dot- multiplying both sides of the above equation by fa. 
Likewise, we solve for tp by dot-multiplying both sides of the same equation 
by Oa, thus obtaining 



-rda = c • fa + • fa 

dip = c ■ + (^{bi ■ — d) 

Now, by recalling the expressions derived above for uj and c, we obtain 
c • fa = -ay(di - da) cos - y(di + da) sin 

c • ea = -ay(di - da) sin + y(di + da) cos 

i • fa = — cos 'ip, i • 6a = — sin ip 



Therefore, 



O3 

iP 



acosip{ 0 i - da) + y(sin V’)(di + da) 



-(asin + d)(di - da) + y(cos V’)(di + da) 



(8.126a) 

(8.126b) 



with the definitions given below: 

a + 6 

I 




r 



P = 



a = 



(8.127) 
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Hence, if we let = [9^ 4’V' be the vector of unactuated joint rates, 

then we have 

6u = ®0a (8.128a) 

with 0 defined as 

acos-^ + (sin'0)/2 — acos-^ + (sin-0)/2 /'qioqkI 

~ yo[— asinV’ + (cos-0)/2 — (5] yo[o!sin'0 + (cos-0)/2 + (5] 

thereby completing the intended kinematic analysis. 

8.6.2 Robots vnth Omnidirectional Wheels 

In general, omnidirectional wheels (ODWs) allow for two independent trans- 
lational motions on the supporting floor and one independent rotational 
motion about a vertical axis. Based on the shapes of the wheels, moreover, 
ODWs can be classified into spherical wheels and Mekanum wheels, the 
latter also being known as ilonators. Spherical wheels have been exten- 
sively investigated in recent years (West and Asada, 1995). We focus here 
on ODWs of the Mekanum type and assume that the robot of interest is 
equipped with n of these. 

The Mekanum wheel bears a set of rollers mounted along the periphery 
of the wheel hub at a given angle, as shown in Figs. 1.11a and 8.24a. 
Furthermore, the rollers are shaped so that the wheel appears as circular 
on its side view, as shown in Fig. 8.24b, in order to ensure a smooth motion. 
Pairwise orthogonal unit vectors e*, fj and gj, hj are defined on the middle 
horizontal planes of the wheel hub and of the roller in contact with the floor, 
respectively. This roller is termed active in the discussion below. Now we 
aim at finding the kinematic relation between the wheel joint rates { 9i }" 
and the Cartesian velocity variables of the robot, namely, the scalar angular 
velocity uj and the 2-dimensional velocity vector c of the mass center of the 




(a) (b) 



FIGURE 8.24. (a) The Mekanum Wheel; (b) its side view. 
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a 

FIGURE 8.25. The active roller of the ith wheel. 

platform. To this end, we express the velocity 6j of the centroid Oj of the 
*th wheel in two different forms: first we look at this velocity from the 
active roller up to the centroid O*; then, from the mass center C of the 
platform to Oi. 

If we relate the velocity of Oi with that of the contact point of the active 
roller with the ground, then we can write, with the aid of Fig. 8.25, 

6i=Pi+Vj (8.129) 

with Vj defined as the relative velocity of Oi with respect to Pi. Now let 

and denote the angular velocity vectors of the hub and the roller, 

respectively, i.e.. 

We thus have 

t>i = U)r X QiPi = (t^k + OiBi + X 6k 

where 6 is the radius of the rollers. In addition, Oi denotes the rate of the 
wheel hub, while <))j denotes that of the active roller, which are positive in 
the directions of vectors e* and gj, respectively. Hence, 

Pi = -b{0ifi + (j>ihi) (8.130) 

Moreover, 

\i=u)hX PiOi = (u;k + OiBi) X (a - 6)k 

a denoting the height of the axis of the wheel hub, as shown in Fig. 8.24b. 
Thus, 

Vi = - 




0i{a - 6)fj 



(8.131) 
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FIGURE 8.26. The layout of the fth wheel with respect to the robot platform. 

thereby obtaining the desired expression for 6j, namely, 

6j = -aOifi - h^ih.i (8.132) 



A general layout of the *th ODW with roller axes at an angle Oj with 
respect to the normal e* to the middle vertical plane of the corresponding 
hub is shown in Fig. 8.26. The subscript i is associated with both the *th 
wheel and its active roller. Moreover, the velocity of the *th wheel, 6j, can 
be expressed in terms of the Cartesian velocity variables, c and uj, as 

6j = c + ct;Edj (8.133) 

where we have used a 2-dimensional vector representation, with dj defined 
as the vector directed from point C to the centroid Oi of the hub and 
E defined as in eq.(4.100). Furthermore, since all rollers are unactuated 
and they rotate idly, the value of <))j is immaterial to our study. Flence, we 
eliminate this variable from the foregoing equations, which is done by dot- 
multiplying both sides of eq.(8.132) by gj, normal to hj, thereby deriving 

gfoi = -a^gffi 



But 

gf fj = sin Ui 

Therefore, 

gfdj = -a(sinaj)6»j 

The same multiplication performed on eq.(8.133) yields 
gfoi = (gfEdi)u; + gfc 



(8.134) 

(8.135) 



Upon equating the right-hand sides of eqs.(8.134) and (8.135), we derive 
the desired relation, namely, 

— a(sino!j)<lj = kft, * = !,..., 



n 



(8.136) 
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where the 3-dimensional vector kj is defined as 

k- = [ ' 

* !_ gi 

and the twist vector t is as defined in eq.(8.115g). We now define the vector 
of wheel rates 9 in the form 

e = [ei 02 ■■■ (8.137) 

If the n equations of eq.(8.136) are now assembled, we obtain 

J9 = Kt (8.138) 

where if we assume that all angles Oj are identical and labeled a, the nxn 
Jacobian J and the n x 3 Jacobian K take the forms 



J = — asinal 


(8.139a) 


■gfEdi gf 




K = 


(8.139b) 


_g^Ed„ g^_ 





with 1 denoting the nxn identity matrix. 

Given eqs.(8.139a) and (8.139b), the differential inverse kinematics can 
be resolved as 

9= Kt (8.140) 

asm a 

whence it is apparent that sin a must be different from zero, i.e., the axes 
of the rollers must not be parallel to the axis of the hub. If these axes are 
parallel, then the ODWs reduce to conventional wheels. 

On the other hand, the twist can be obtained from eq.(8.138), for n = 3, 
as 

t = (8.141) 

where can be found in closed form as 

K-^ = -\ gfEgg g^Egi 1 /Qi42ai 

A l_E(r2g3 -T3g2) E(r3gi-rig3) E(rig 2 - r 2 gi) J 
with A and { r* }?( defined as 

A = det(K) = rigjEg 2 + r 2 gf Eg 3 + r 3 g^Egi (8.142b) 
ri=gfEdi, * = 1,2,3 (8.142c) 



which thus completes the kinematic analysis of the system at hand. 




9 

Trajectory Planning: Continnons-Path 
Operations 



9.1 Introduction 

As a follow-up to Chapter 5, where we studied trajectory planning for pick- 
and-place operations (PPO), we study in this chapter continuous-path op- 
erations. In PPO, the pose, twist, and twist-rate of the EE are specified only 
at the two ends of the trajectory, the purpose of trajectory planning then 
being to blend the two end poses with a smooth motion. When this blend- 
ing is done in the joint- variable space, the problem is straightforward, as 
demonstrated in Chapter 5. There are instances in which the blending must 
be made in Cartesian space, in which advanced notions of interpolation in 
what is known as the image space of spatial displacements, as introduced 
by Ravani and Roth (1984), are needed. The image space of spatial dis- 
placements is a projective space with three dual dimensions, which means 
that a point of this space is specified by four coordinates — similar to the 
homogeneous coordinates introduced in Section 2.5 — of the form Xj + 
for i = 1,2, 3,4, where e is the dual unity, which has the property that 
= 0. The foregoing coordinates are thus dual numbers, their purpose 
being to represent both rotation and translation in one single quantity. In 
following Ravani and Roth’s work, Ge and Kang (1995) proposed an inter- 
polation scheme that produces curves in the image space with second-order 
geometric continuity, which are referred to as curves. These interpola- 
tion techniques lie beyond the scope of the book and will be left aside. The 
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interested reader will find a comprehensive and up-to-date review of these 
techniques in (Srinivasan and Ge, 1997). 

The purpose of this chapter is to develop motion interpolation techniques 
in Cartesian space that produce smooth motions in both Cartesian and joint 
spaces. Motion interpolation in joint space was discussed in Chapter 5, the 
present chapter being devoted to motion interpolation in Cartesian space. 
To this end, we resort to basic notions of differential geometry. 



9.2 Curve Geometry 

Continuous-path robotics applications appear in operations such as arc- 
welding, flame-cutting, deburring, and routing. In these operations, a tool 
is rigidly attached to the end-effector of a robotic manipulator, the tool be- 
ing meant to trace a continuous and smooth trajectory in a 6-dimensional 
configuration space. Three dimensions of this space describe the spatial 
path followed by the operation point of the EE, while the remaining three 
describe the orientation of the EE. Some applications require that this task 
take place along a warped curve, such as those encountered at the inter- 
sections of warped surfaces, e.g., in aircraft fuselages, while the path is to 
be traversed as a prescribed function of time. This function, moreover, is 
task-dependent; e.g., in arc-welding, the electrode must traverse the path 
at a constant speed, if no compensation for gravity is taken into account. 
If gravity compensation is warranted, then the speed varies with the ori- 
entation of the path with respect to the vertical. Below we will define this 
orientation as that of the Frenet-Serret frame associated with every point 
of the path where the path is smooth. 

Moreover, for functional reasons, the orientation of the EE is given as a 
rotation matrix that is, in turn, a prescribed smooth function of time. In 
arc-welding, for example, the orientation of the electrode with respect to 
the curve must be constant. The trajectory planning of the configuration 
subspace associated with the warped path is more or less straightforward, 
but the planning of the trajectory associated with the orientation subspace 
is less so. 

While most methods of trajectory planning at the Cartesian-coordinate 
level focus on the path followed by the operation point, the underling in- 
verse kinematics of a six-axis robotic manipulator requires the specification 
of the orientation of the EE as well. In the presence of simple manipulators 
with a spherical wrist, as those studied in Subsection 4.4.2, the positioning 
and the orientation tasks are readily separable, and hence, the planning of 
the two tasks can be done one at a time. In other instances, e.g., in most 
arc-welding robots, such a separation is not possible, and both tasks must 
be planned concurrently, which is the focus of our discussion below. Here, 
we follow the technique presented in (Angeles et ah, 1988). 

Crucial to our discussion is the concept of path orientation. Let T be a 
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warped curve in 3-dimensional space that is smooth in a certain interval of 
interest for our discussion. Under these conditions, we can associate with 
every point of this interval an orthonormal triad of vectors, i.e., a set of unit 
vectors that are mutually orthogonal, namely, the tangent, the normal, and 
the binormal vectors of F. Therefore, when this set of vectors is properly 
arranged in a 3 x 3 array, a rotation matrix is obtained. This matrix thus 
represents the orientation of F. In order to parameterize these vectors, let 
s be the arc length measured along F from a certain reference point on this 
curve. Below we review the basic differential-geometric concepts pertaining 
to our discussion. 

The tangent, normal, and binormal unit vectors, ej, e„, and 65 , respec- 
tively, associated with every point of F where this curve is smooth, are 
generically termed here the Frenet-Serret vectors. These vectors are de- 
fined as 



©t = r' 


(9.1a) 


r' X r" 




— II , .. II 


(9.1b) 


r' X r'' 






(9.1c) 



where r' stands for dr/ds and r" for d'^r/ds'^. Now the Frenet-Serret re- 
lations among the three foregoing unit vectors and the curvature k and 
torsion t of F are recalled (Brand, 1965): 



det 

d,s 

d,s 

deb 

d,s 



KB„ 

-net + t ©6 

-T©„ 



(9.2a) 

(9.2b) 

(9.2c) 



Moreover, the curvature and torsion can be calculated with the aid of the 
formulas 



K 

T 



||r' X r"|| 
r' X r" • r"' 



(9.3a) 

(9.3b) 



where r"' stands for d^r/ds^. Furthermore, differentiation of k and t, as 
given above, with respect to s, yields 



k'(s) 

t'{s) 



(r^ X r'' 



(r^ X r" 



r X r 



K 

// , ■p(iu) _ 



2r(r' X r") • (F x F' 



(9.4a) 

(9.4b) 



where stands for d'^r/ds'^. The geometric interpretation of the curva- 
ture is the rate of change of orientation of the tangent vector with respect 
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to the arc length; that of the torsion is the rate at which the curve quits the 
plane of the tangent and normal vectors. Thus, at points where the curva- 
ture vanishes, the curve approximates a line to a second order, i.e., up to 
second-order derivatives, whereas at points where the torsion vanishes, the 
curve approximates a planar curve to a third order — Notice that the torsion 
involves third-order derivatives. Now, from the Ffenet-Serret formulas and 
the chain rule, we can derive the time-rate of change of the Ffenet-Serret 
vectors, namely. 



et = 



— 



65 = 



det . 

— S= S5C6„ 

as 


(9.5a) 


de^i 


(9.5b) 


— — s= —sKOt + 
as 


deb . 

— — S= -ST6„ 

as 


(9.5c) 



Furthermore, let oj be the angular velocity of the Frenet-Serret frame. Then, 
clearly. 



et = io X et (9.6a) 

e„ = w X e„ (9.6b) 

65 = tU X 65 (9.6c) 

Upon equating pairwise the right-hand sides of eqs.(9.5a-c) and eqs.(9.6a- 
c), we obtain three vector equations determining tu, namely, 

— Ejtu = S 5 C 6 „ (9.7a) 

— E„tu = — s5C6t + ST 65 (9.7b) 

— E 5 W = — ST 6 „ (9.7c) 

where we have introduced the cross-product matrices Ej, E„, and E5 of 
vectors et, 6 „, and 65 , respectively, thereby obtaining a system of nine 
scalar equations in the three unknown components of tu, i.e., 

Auj = b (9.8a) 



with A defined as the 9x3 matrix and b as the 9-dimensional vector 
displayed below: 





'Et' 




SKBn 


A = - 


E„ 


, b = 


s{-Ket + T 65 ) 








-ST 6 „ 



(9.8b) 



Although the foregoing system is overdetermined, it is consistent, and hence 
it comprises exactly three linearly independent equations, the remaining six 
being dependent on the former. One way to reduce system (9.8a) to only 
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three equations consists in multiplying both sides of this equation by A^. 
Now, the product A^A greatly simplifies because matrix A turns out to 
be isotropic, as per the discussion of Section 4.9, i.e., its three columns 
are mutually orthogonal and all have the same magnitude. This fact can 
become apparent if we realize that the three 3x3 blocks of A are cross- 
product matrices of three orthonormal vectors. Thus, 

A^A = EfEt + E^E„ + E^E5 

If we now recall Theorem 2.3.4, the foregoing products take on quite simple 
forms, namely. 



EfE* = -E2 = -(-l + e*ef) 

E^Es = -Ej = -(-1 + e^ej) 

Moreover, for any 3-dimensional vector v, we have 
(etef + e„e^ + ebe[)v = v 

and hence, the above sum in parentheses reduces to the identity matrix, 
i.e., 

etef + e„e^ + e^el = 1 
the product A^A thus reducing to 

A^A= (2)1 

Therefore, u> takes on the form 



(jj 




E„ 



Eb 



sKe„ 

S{-Ket + TBb) 

-sre„ 



or upon expansion, 

g 

w = - [net X e„+e„x {tbi, - net) - tbi, x e„] (9.10) 

However, since the Ffenet-Serret triad is orthonormal, we have 

et X e„ = 66 , e„X 66 = et, 65 x et = e„ (9-11) 

Upon substitution of expressions (9.11) into the expression for u> given in 
eq.(9.10), we obtain 

(jj = s5 (9.12) 

where 5 is the Darboux vector, defined as 



S = TBt + KBb 



( 9 . 13 ) 
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Expressions for the curvature and torsion in terms of the time-derivatives 
of the position vector are readily derived using the chain rule, which leads 
to 



K 



T 



l|r X r|| 

||r||3 

r X r- if 

llr X rip 



(9.14a) 

(9.14b) 



Upon differentiation of both sides of eq.(9.12), the angular acceleration 
u) is derived as 



th = + s8 (9.15) 

where the time-derivative of the Darboux vector is given, in turn, as 

5 = fet + kob (9.16) 

in which eqs.(9.5a-c) have contributed to the simplification of the above 
expression. The time-derivatives of the curvature and torsion are readily 
derived by application of the chain rule, thereby obtaining 



k = sk'{s)= — (r' X r"') • (r' x r") 

K 



f = st'(s)= X r" ■ — 2 r(r' x r'") • (r' x r")l 

The time-derivative of the Darboux vector thus reduces to 

d = s(Aet + Beb) 

where scalars A and B are computed as 

^ _ r' X r" • - 2 r(r' x r'") • (r' x r") 

^ = 2 

(r' X r "0 • (U X r") 

JD = 



and hence, the angular acceleration reduces to 

th = + s‘^{Aet + Beb) 



(9.17a) 

(9.17b) 



(9.18a) 



(9.18b) 

(9.18c) 



(9.19) 



Prom the relations derived above, it is apparent that the angular velocity 
is a bilinear function of the Darboux vector and s, while the angular accel- 
eration is linear in s and quadratic in s. The computational costs involved 
in the calculation of the angular velocity and its time-derivative amount 
to 31 multiplications and 13 additions for the former, and 28 multiplica- 
tions with 14 additions for the latter (Angeles et ah, 1988). Notice that the 
angular velocity requires, additionally, one square root. 
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In the above discussion, it is assumed that explicit formulas for the two 
time-derivatives of the arc length s are available. This is often not the case, 
as we show with the examples below, whereby an intermediate parameter, 
which is easier to handle, is introduced. What we will need are, in fact, 
alternative expressions for the quantities involved, in terms of kinematic 
variables; i.e., we need time-derivatives of the position vector r rather than 
derivatives of this vector with respect to the arc length s. Below we derive 
these expressions. 

First, note that ej can be obtained by simply normalizing the velocity 
vector r, namely, as 



l|r|| 


(9.20) 


where it is not difficult to realize that 




s = l|r|l 


(9.21) 


Moreover, the binormal vector 65 can be derived by application of the chain 
rule to vector r', namely. 


„ dr' dr' / dt 1 d ^ 

ds ds/dt s dt ^ 


(9.22a) 


But 

, dr r 

r{s) = — = - 
as s 


(9.22b) 


and hence, 





s 

Now, upon substitution of expressions (9.22b & c) into eq.(9.fb), an al- 
ternative expression for 65 is derived, in terms of time-derivatives of the 
position vector, namely. 



d 

dt 



(9.22c) 



65 



r X r 
r X r 



(9.23) 



Finally, e„ can be readily computed as the cross product of the first two 
vectors of the Ffenet-Serret triad, namely. 



Gn — ^ 



(r X r) X r 
||r X r|| ||r|| 



(9.24) 



The time-derivatives of the Ffenet-Serret vectors can be computed by 
direct differentiation of the expressions given above, namely, eqs.(9.20), 
(9.23), and (9.24). 
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9.3 Parametric Path Representation 

Only seldom is an explicit representation of the position vector r of a ge- 
ometric curve possible in terms of the arc length. In most practical cases, 
alternative representations should be used. The representation of the posi- 
tion vector in terms of a parameter <t, whatever its geometric interpretation 
may be, whether length or angle, will henceforth be termed a parametric 
representation of the curve at hand. The choice of a is problem-dependent, 
as we illustrate with examples. 

Below we derive expressions for (a) the Ffenet-Serret triad; (b) the cur- 
vature and torsion; and (c) the derivatives of the latter with respect to 
the arc length. All these expressions, moreover, will be given in terms of 
derivatives with respect to the working parameter a. The key relation that 
we will use is based on the chain rule, already recalled several times earlier. 
Thus, for any vector v(<t). 



dv dv da 

ds da ds 

However, the foregoing relation is not very useful because we do not have 
an explicit representation of parameter a in terms of the arc length. Never- 
theless, we will assume that these two variables, s and a, obey a monotonic 
relation. What this means is that 



da 

ds 



> 0 



(9.25) 



which is normally the case. Under this assumption, moreover, we can write 
the derivative of v as 

dv cJn j da 

d,s ds/da 



where, apparently. 



ds 

da 



dr 

da 



l|r'(^)ll 



Therefore, the derivative sought takes the form 



dv v'(cr) 
ds ||r'(cr)|| 



(9.26a) 



It goes without saying that the same relation holds for scalars, i.e.. 



dv v'{a) 
ds ||r'(cr)|| 



(9.26b) 



Expressions for the Ffenet-Serret triad now follow immediately, i.e.. 



l|r'(cr)|| 



(9.27a) 
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v'{a) X r"((r) 
l|r'(cr) X r"(cr)|| 

[r'((r) X r"((r)] x r'{a) 
||r'(cr) X r"(cr)||||r'(cr)|| 



(9.27b) 

(9.27c) 



Now, paraphrasing relations (9.14a & b), we have 



||r'(g) X r"(g)|| 
||r'(a)||3 

r'(cr) X r"(cr) • r'" 
||r'((T) X r"((r)P 



(9.28a) 

(9.28b) 



the partial derivatives of the curvature and torsion with respect to the arc 
length being computed in terms of the corresponding partial derivatives 
with respect to the parameter <t, which is done with the aid of the chain 
rule, i.e.. 



d(s) = 



|r'(c 



l|r'(cr)|| 



(9.29) 



Expressions for k'(<t) and T'{a), in turn, are derived by a straightforward 
differentiation of the expressions for k and t in terms of a, as given in 
eqs.(9.28a & b). To this end, we first recall a useful expression for the 
derivative of a rational expression q{x) whose numerator and denominator 
are denoted by N{x) and D{x), respectively. This expression is 



q'{x) 



[N'{x)-q{x)D'{x)] 

D[x) 



(9.30a) 



Note that nothing prevents the numerator of the foregoing rational expres- 
sion from being a vector, and hence, a similar formula can be applied to 
vector ratios as well. Let the denominator of a vector rational function q(x) 
be n(x). Under these conditions, then, we have 

“ q(a;)£''(a:)] (9.30b) 

D[x) 

As a matter of fact, the above relation can be extended to matrix numer- 
ators. Not only is this possible, but the argument can likewise be a vector 
or a matrix variable, and similar formulas would apply correspondingly. 
We thus have, for the derivative of the curvature. 



k'(ct) 



|r'(c 



^||r'(cr) X r"(cr)|| - K^\\r'{a)f 
da da 



(9.31) 



Now we find the first term inside the brackets of the foregoing expression 
from the relation 
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which yields 



(kr 



2||r' X r"|| da 



|r'((r) X r”{a 






But 



^||r'(cr) X r"(cr)f = ^{[r'(cr) X r"(cr)] • [r'(cr) X r"(cr)]} 

= 2[r'(cr) X r"(cr)] • ^[r'(cr) x r"(cr)] (9.32) 

the derivative of the above term in brackets reducing to 

— [r'((r) X Y"(a)] = r' (a) x Y'"(a) 
da 

and hence, 



da 

Furthermore, 



d ^ ^ [r'(g) X r"(g)] ■ [r'(g) x v"'{a)] 



I r' X r" 



^||r'(a)f = 3||r'(a)f^||r'(a) 



(9.33a) 



the last derivative again being found from an intermediate relation, namely, 

^||r'(a)f = 2||rV)ll;^l|r'(a)|| 
da da 



whence. 



with 



and so. 



Therefore, 






V„M|2 



r'{a 



^l|r'(cr)f = = 2r'(cr) • r"(cr) 



d , r'{a) • r"{a) d , 2 

||F(a)|| 



-||r'(a)f = 3||rV)l|r'(a).r"(a) 



(9.33b) 



Substitution of eqs. (9.33a & b) into eq.(9.3f) yields the desired expression, 
namely, 



^ [r'(g) X r"(g)] ■ [r'(g) x v'''{a)] _ ' r"(cr) 



|r'((T)|P||r' X r"ll 



|r'(c 



(9.34) 
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Likewise, 

t'(ct) = ^ (9.35a) 

with N and D defined as 

N = — [r'((r) X r"(a) ■ Y"'(a)] — t— ||r'(<T) x r"(<T)|p (9.35b) 

da da 

D = ||r'((T) X r"((r)|p (9.35c) 



The first term of the numerator N of the foregoing expression can be readily 
calculated as 

— [rTcr) X r"(a) ■ Y"'(a)] = rTa) x r'Ta) • (9.35d) 

da 

while the derivative appearing in the second term of the same numerator 
was obtained previously, as displayed in eq.(9.32). Upon substitution of the 
expressions appearing in eqs.(9.32) and (9.35d) into eq. (9.35a), we obtain 
the desired expression: 



r'{a) X r"((r) • [r’^®'“)((r) — 2rr'(a) x r'''{a)\ 
||r'((T) X r"((r)P 



thereby completing the desired relations. 



Example 9.3.1 (Planning of a gluing operation) A robot used for a 
gluing operation is required to guide the glue nozzle fixed to its end-effector 
through a helicoidal path so that the tip of the nozzle traverses the helix at 
a constant speed vg = 0.8m/s and the end-effector maintains a fixed orien- 
tation with respect to the curve, i.e., with respect to the Frenet-Serret triad 
of the helix. Determine the orientation matrix Q of the end- effector with 
respect to a frame {x, y, z} fixed to the robot base, as well as the angular 
velocity and angular acceleration of the end- effector. The operation is to be 
performed with a Fanuc S-300 robot, whose Denavit-Hartenberg (DFl) pa- 
rameters are given in Table 9.1, while the axis of the helix is chosen to be 
parallel to the first axis of the robot and beginning at point Pg (2, —2, 1.2) 
in meters. Find the joint trajectories of the robot as well as the associated 
joint rates and joint accelerations from Cartesian position, velocity, and 
acceleration data. Verify that the joint-rate and joint- acceleration profiles 
are compatible with those of the joint variables. It is known that the radius 
of the helix is a = l.d m and that its pitch is b = 2.5 m/turn. Finally, the 
gluing seam spans through one quarter of a helix turn. 



Solution: We will use a Cartesian frame fixed to the base of the robot such 
that its z axis coincides with the axis of the first revolute. The helix can 
then be given in the parametric representation shown below: 



x 

y 

z 








Link 


a,i (m) 


hi (m) 


(deg) 


1 


0.0 


0.9 


90 


2 


0.9 


0.0 


0 


3 


0.95 


0.0 


90 


4 


0.0 


1.3 


-90 


5 


0.0 


0.0 


90 


6 


0.0 


0.44 


-90 
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FIGURE 9.2. Joint velocities for a Fanuc S-300. 

where the parameter cp is the angle made by the projection, onto the X-Y 
plane, of the position vector of a point P of the helix with the x axis. In 
the process, we will need first and second time-derivatives of the foregoing 
Cartesian coordinates. These are given below for quick reference: 



X = —acp sirup 



y 

z 



ap cos p 

b . 



X 

y 

z 



—ap cosp — apsmp 
—ap sm p Yap cos p 



27T 






and 
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We now impose the constant-speed condition, which leads to 



62 



•2 I -2 I -2 _ 2 -2 I -2 

X +y + z =a if + —if 



V 



2 

0 



and hence, 



(f = c 



where the constant c is defined as 



c = -Co 



47t2 

47r2a2 + 62 



Thus, is constant, and hence. 



If = ct 

Moreover, in terms of constant c, the Cartesian coordinates of a point of 
the helix take on the forms 



X = 2 -\- a cos ct 
y = —2 + asinct 
he 

z = 1.2 H 1 

2n 

the first time-derivatives of these coordinates becoming 

X = —ac sin ct 
y = ac cos ct 
6c 

and the corresponding second time-derivatives 

it = —ac?" cos ct 
y = —ac} sin ct 

z = 0 



Now the Ffenet-Serret triad is readily calculated as 

dr r 



Furthermore, 



* ds s vq 



— asinct 
a cos ct 
6/27T 



det 

ds 



2i 

s 



ac 

„,2 



— COS ct 

— sin ct 

0 



= 
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from which it is apparent that 

c? _ 47T^a 

Thus, the binormal vector 65 is calculated simply as the cross product of 
the first two vectors of the Prenet-Serret triad, namely, 

— (6/27t) sin ct 
(6/27t) cos ct 
—a 

and hence, the orientation matrix Q of the gluing nozzle, or of the end- 
effector for that matter, is given by 

Q = [ et e„ 66 ] 

Hence, 

— asinct —(uq/c) coset (6/27 t) sin ct 
Q= — acoset — (co/c)sinct —{b/2n)cosct 

(b/2Tr) 0 a 

Now, the angular velocity is determined from eq.(9.12), which requires the 
calculation of the Darboux vector, as given in eq.(9.13). Upon calculation 
of the Darboux vector and substitution of the expression thus resulting into 
eq.(9.12), we obtain 



c 

65 = X 6^ — 

Vo 



cos ct 
sin ct 
0 



0 




'o' 


0 


= c 


0 


(47r2a2 -\- 62)/4t62 




1 



which is thus constant, and hence, 

th = 0 



Now, the coordinates of the center of the wrist, C, are determined with 
the aid of relation (4.18c), where the operation point is a point on the 
helix, i.e., p = xi + yj + zk, parameters b^, Ae, and jiQ being obtained from 
Table 9.1, namely. 



&6 = 0.440 m, Ae = cosoe = 0, p,6 = sinoe = — 1 

Furthermore, the numerical value of c is obtained from the helix geometry, 
namely. 



c 



0.8 



47^2 

47t2 X 1.02-9 2.52 



0.48522 s^i 
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FIGURE 9.3. Joint accelerations for a Fanuc S-300. 



Upon substitution in eq.(4.18c) of the entries found above for Q, along with 
the numerical values, we obtain the Cartesian coordinates of the center C 
of the spherical wrist of the robot as 



xc 




■ 2+ 1.16cos(0.48522t) ' 


Vc 


= 


-2 + 1.16sin(0.48522t) 


_zc _ 




1.2 + 0.193067 



in meters. Apparently, point C describes a helicoidal path as well, although 
of a smaller radius, that is coaxial with the given helix. 

Now the time- histories of the joint angles are computed from inverse 
kinematics. Note that the robot at hand being of the decoupled type, it 
allows for a simple inverse kinematics solution. The details of the solution 
were discussed extensively in Section 4.4 and are left as an exercise to the 
reader. 

Of the four inverse kinematics solutions of the arm, three were found to 
lead to link interferences, when these trajectories were tested with the aid 
of RVS, the package for robot visualization developed at McGill University 
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(Darcovich et al., 1999). Hence, only one such solution is physically pos- 
sible. This solution, along with one of the two wrist solutions, is plotted 
in Fig. 9.1, with Figs. 9.2 and 9.3 showing, respectively, the corresponding 
joint rates and joint accelerations. 

Note that the maxima and minima of the joint- variables occur at instants 
where the corresponding joint rates vanish. Likewise, the maxima and min- 
ima of joint rates occur at instants where the associated joint accelerations 
vanish, thereby verifying that the computed results are compatible. A more 
detailed verification can be done by numerical differentiation of the joint- 
variable time- histories. 

Example 9.3.2 (Planning of an arc-welding operation) A spherical 
reservoir of radius R is to he arc-welded to a cylindrical pipe of radius r, 
with the axis of the cylinder located a distance d from the center of the 
sphere, all elements of the cylinder piercing the sphere, i.e., d r < R, 
as shown in Fig. 9.f. Note that two intersection curves are geometrically 
possible, hut the welding will take place only along the upper curve. More- 
over, the welding electrode is to traverse the intersection curve, while the 
tool carrying the electrode is to keep a constant orientation with respect to 
that curve. In the coordinate frame shown in Fig. 9.f, find an expression 
for the rotation matrix defining the orientation of the end-effector, to which 
the electrode is rigidly attached. 

Solution: Note that the X axis of the coordinate frame indicated in Fig. 9.4 
intersects the A axis of the cylinder, this axis being parallel to the Z axis. 
Moreover, we define as the angle shown in Fig 9.4b. Now, the x and y 




(a) 



(b) 



FIGURE 9.4. Intersection curve between a spherical reservoir and a cylindrical 
pipe. 
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coordinates of an arbitrary point of the intersection curve are given by 

x = d + rcos(p (9.36a) 

y = rsiny> (9.36b) 

Further, in order to find the remaining z coordinate, we use the equation 
of the sphere, <S, namely, 

<S: +y'^ +z'^ = 



If we substitute the x and y coordinates of the intersection curve in the 
above equation and then solve for the z coordinate in terms of y>, we obtain 

z = ± — d? — 2 dr cos (9.36c) 

In the above relation, the plus and minus signs correspond to the upper 
and lower portions of the intersection curve, respectively. Since we are 
interested in only the upper intersection, we will take only the positive 
sign in that relation. Furthermore, we define 



d = Ar, R = jxr 



where A and p, are nondimensional constants. Moreover, let 

- A^ - 1 > 0 
1 

p = — 

■s/ — 2A cos p 

the inequality following from the geometry of Fig. 9. 4b. Then, the position 
vector r of any point on the intersection curve can be expressed in the form 



r = r 



A + cos p 
sin p 

1/p 



Now, upon differentiation of r with respect to p, we obtain 



r'(p) = r 



— sinp 
cos p 
Ap sin p 



r"(p) = r 



— cos p 

— sin p 

Ap cos p — (A^ sin^ 



(9.37) 



(9.38a) 

(9.38b) 



where we have used the relation 



p'(p) = — (Asinp)p® 
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In addition, using eqs.(9.38a & b), we derive the items needed to compute 
the Prenet-Serret triad, from which we will derive the required orientation 
matrix, i.e.. 



r'(p) X r"(^) 
l|r'(p)|| 

l|r'(p)xr"(^)|| 



\if — cos ip sin^ p 
1 



‘ (p® sin® p 



rG{p) 

r‘^p^\/D{p) 



(9.39a) 

(9.39b) 

(9.39c) 



with functions D{p) and G(p) defined as 



D = + A*^ + yO® — 6y0® A(A® + yO®) cos p + 6A®(A® + 2y0®) cos® p 

+ 2A® (yO® — 4) cos® (yC — 3 A*^ cos*^ (yC (9.39d) 

G = y^l + A2(y3>2 sin® p (9.39e) 

Now 6t, 66, and e„ are obtained as 



et = 



65 = 



— 



l|r'(p)|| 



1 

G 



— sin p 
cos p 
Xp sin p 



G 



r'{p) X r"{p) 
l|r'(^) X r"(^)|| 



1 

p^\fT) 



Xp — A®(y5® cos p sin® p 
— A®95® sin® p 
1 



1 

P^VDG 



— A®(y5‘^ sin*^ p — cos p 
A®(y5‘^ cos p sin® p — 'X}p^ sin p — sin p 
Xp cos p — X? if ^ sin® p 



n6 

p^\Td 



n„ 

p^Vdg 



(9.40a) 

(9.40b) 

(9.40c) 



where e„ has been calculated as e„ = 65 x ej. 

The orthogonal matrix defining the orientation of the end-effector can 
now be readily computed as 



Q = [et e„ 65 ] 

for we have all the necessary expressions. Note, however, that these expres- 
sions allow us to find Q for any value of p, but we do not have, as yet, an 
expression of the form p{t) that would allow us to obtain Q(t). Such an 
expression is derived in Example 9.5.1. 

Example 9.3.3 (Calculation of torsion, curvature, and Darboux 
vector) ITe refer here to the intersection curve of Example 9.3.2, for which 
we want to find expressions for its curvature, torsion, and Darboux vector. 
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Solution: We can use directly the expressions derived above, eqs. (9.28a & 
b), to obtain the curvature and torsion in terms of derivatives with respect 
to parameter y>. With these expressions and those for the Ffenet-Serret 
triad, the Darboux vector would follow. However, we can take shortcuts, 
for we already have expressions for the Prenet-Serret triad, if we express 
the curvature and torsion in terms of this triad and its derivatives with 
respect to y>, as we explain below. Indeed, from the Ffenet-Serret relations, 
eqs. (9.2b), we can express the curvature and torsion in the forms 

K = e((s) • e„ (9.41a) 

T=-e(,(s)-e„ (9.41b) 

and hence, all we need now are the derivatives of the tangent and normal 
vectors with respect to s. These are readily derived using relation (9.26a), 
i.e.. 



ej(«) 



e^(«) 



e)(y) 

l|r'(^)|| 

e),(y) 

l|r'(^)|| 



(9.42a) 

(9.42b) 



Now, in order to differentiate the Ffenet-Serret triad with respect to 
y>, we first note, from eqs.(9.40a-c), that these three expressions are vector 
rational functions, and hence, their derivatives with respect to are derived 
by applying eq. (9.30b), thereby obtaining 



e'tiv) 









(9.43) 

(9.44) 



where rij and Ub are the numerators of the vector rational expressions of 
Bt and 66, respectively, given in eq. (9.40a & b). Below we calculate the 
foregoing derivatives with respect to y>: 



n)(^) 



— COS (f 

— sin (f 

\0{cos sin^ y>) 



ri6(^) 



A 



ip' — \ip^ sin y>[3(p' cosy sin + y(3cos^ y — 1)] 
— 3Ay^ sin^ y [y' sin y + y cos y] 

0 



— Asiny 

(p2 — 2Acosy)3/2 



y' = y'(y) 
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= 6yO^A(A^ + p^) sinc^ — 12A^(A^ + 2p^) cosc^sinc^ 
— 6 A^(yO^ — 4) cos^ (fsmp + 12A*^ cos® (fsmp 



G'{p) 



A® sin If 

2G 



[2f cos f + f' sin f) 



and ||r'((^)|| was already calculated in Example 9.3.2. 

If we now substitute all the foregoing expressions into eqs. (9.42a & b), 
we obtain, after intensive simplifications, 



K 

T 



G^r 

X^fEsmif 

rDG^ 



(9.45a) 

(9.45b) 



with function E{f) defined, in turn, as 

E(f) = A®(p*^ sin*^ f + Ay>® sin® fiXcosf — 1) + cosy] (9.46) 

y4 

With the foregoing expressions for ej, 65 , t, and k, computing the Darboux 
vector of the intersection curve reduces to a routine substitution of the 
foregoing expressions into eq.(9.13). 



9.4 Parametric Splines in Trajectory Planning 

Sometimes the path to be followed by the tip of the end-effector is given 
only as a discrete set of sampled points {Pj}]^. This is the case, for example, 
if the path is the intersection of two warped surfaces, as in the arc- welding 
of two plates of the hull of a vessel or the spot- welding of two sheets of the 
fuselage of an airplane. In these instances, the coordinates of the sampled 
points are either calculated numerically via nonlinear-equation solving or 
estimated using a vision system. In either case, it is clear that only point 
coordinates are available, while trajectory planning calls for information on 
derivatives of the position vector of points along the path with respect to the 
arc length. These derivatives can be estimated via a suitable interpolation 
of the given coordinates. Various interpolation schemes are available (Foley 
and Van Dam, 1982; Hoschek and Lasser, 1992), the most widely accepted 
ones being based on spline functions, which were introduced in Section 5.6. 
The splines introduced therein are applicable whenever a function, not a 
geometric curve, is to be interpolated. However, in trajectory planning, 
geometric curves in three-dimensional space come into play, and hence, 
those splines, termed nonpararnetric, are no longer applicable. What we 
need here are parametric splines, as described below. 
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Although parametric splines, in turn, can be of various types (Dierckx, 
1993), we will focus here on cubic parametric splines because of their sim- 
plicity. 

Let Pi{xi, yi, Zi), for * = 1, . . . , A^, be the set of sampled points on the 
path to be traced by the tip of the end-effector, {Pjf being the set of 
corresponding position vectors. Our purpose in this section is to produce 
a smooth curve T that passes through and that has a continuous 

Ffenet-Serret triad. To this end, we will resort to the expressions derived 
in Section 9.3, in terms of a parameter a, which we will define presently. 

We first introduce a few definitions: Let the A:th derivative of the position 
vector p of an arbitrary point P of F with respect to a, evaluated at Pi, 
be denoted by its components being denoted correspondingly by 

and Next, the coordinates of P are expressed as piecewise cubic 
polynomials of a, namely, 

x(a) = A.^i(a - aif + - aif + C’a,i(o- - cTj) + (9.47a) 

y{a) = Ayi{a - aif + Byfa - aff + Cyfa - af + Dyi (9.47b) 
z{a) = Azi{a - aif + B^fa - aff + C^fa - af + D^i (9.47c) 

for a real parameter a, such that ai < a < Ci+i, and i = 1, . . . , N — 1, with 
ai defined as 



ai = 0, cTj+i =ai+ Ao-j, Acj = ^ Axf + Ay| + Azf (9.47d) 
Axi = Xj+i - Xi, Ayi = yi^i - yi, Azi = Zj+i - Zj (9.47e) 

and hence, Acj represents the length of the chord subtended by the arc 
of path between Pi and Pj+i. Likewise, a denotes a path length measured 
along the spatial polygonal joining the N points {Pj}(^. Thus, the closer 
the aforementioned points, the closer the approximation of Acj to the arc 
length between these two points, and hence, the better the approximations 
of the curve properties. 

The foregoing spline coefficients A^i, Ayi, . . . , D^i, for * = 1, . . . , A — 1, 
are determined as explained below. Let us define the A-dimensional vectors 



X = [xi, . 


• ■,XNf, 


x" ^ [x'i, . 


..,xX 


(9.48a) 


y = [yi, ■ ■ 


■ ■ ,yNf , 


y"^[y'f-- 




(9.48b) 


z = [zi, . . 




z"^[4,.. 




(9.48c) 



The relationships between x, y, and z and their counterparts x", y", and 
z" are the same as those found for nonparametric splines in eq.(5.58a), 
namely. 



Ax" = 6Cx 
Ay" = 6Cy 
Az" = 6Cz 



(9.49a) 

(9.49b) 

(9.49c) 
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which are expressions similar to those of eq. (5.58a), except that the A 
and C matrices appearing in eq. (9.49b) are now themselves functions of 
the coordinates of the supporting points (SP) of the spline. In fact, the 
{N — 2) X N matrices A and C are defined exactly as in eqs. (5.58b & c), 
repeated below for quick reference: 



A 



ai 2 q : i _2 02 

0 02 202,3 



0 

03 



0 0 - 

0 0 



0 0 
. 0 0 



0 



07V'" 2o7V',"w 

Oat// 



oat" 0 
2oat "]v' o-n' - 



(9.49d) 



and 



r/3i 

0 



c = 



0 

. 0 



■/3 i ,2 /?2 0 

/?2 -/?2,3 fh 



0 • • • Pn'" 

0 0 



■pN',"N' 

pN" 



0 0 - 

0 0 



Pn" 0 

■pN,"N' pN' - 



(9.49e) 



where ak and pk are now defined correspondingly, i.e., for *, j. A: = 1, . . . , N' , 



Ofc ^28 f^k 1/ofc, f^i,j 4^ (9.50) 

while N' , N”, and N”' are defined as in eq.(5.58f), i.e., as 

N' = N-l, N" = N-2, N'" = N-:i (9.51) 

Note that the spline p(cr) is fully determined once its coefficients are 
known. These are computed exactly as their counterparts for nonpara- 
metric splines, namely, as in eqs.(5.55a-e). Obviously, different from the 
aforementioned formulas, the coefficients of the parametric spline pertain 
to three coordinates, and hence, three sets of such coefficients need be com- 
puted in this case. In order to simplify matters, we introduce the vectors 
below: 





^xk 




Bxk 


Sfc = 


-^yk 


, bfc = 


Byk 




^zk 




Bzk 





Cxk 




Dxk 


Cfc = 


Cyk 


, dk = 


Dyk 




Czk 




D zk 



(9.52) 



and thus, the position vector of an arbitrary point P on the parametric 
spline takes on the form 



p(cr) = afc(cr - akf + bfc(cr - akf + Ck{a - ak) + dk, k = 1, . . . ,N - 1 

(9.53a) 
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in the interval Uk < cr < (Tk+i- The counterpart set of eqs.(5.55a-e) is then 



afc = (Pfc+i - Pfc) (9.53b) 

bfc = i pO (9.53c) 

Cfc = ^ AcTfc (pO+ 1 + 2p(() (9.53d) 

dfc = Pfc (9.53e) 

Apfc=pfc+i-pfc (9.53f) 

where vectors pfc and p(( are defined as 





Xk 


. Pfc = 


a^fc 


Pfc = 


Uk 


y'k 

// 




_Zk _ 







Note that since p is piecewise cubic in <t, p' is piecewise quadratic, whereas 
p" is piecewise linear in the same argument, p"' being piecewise constant; 
higher-order derivatives vanish. Properly speaking, however, the piecewise 
constancy of p"' causes the fourth-order derivative to be discontinuous at 
the SP, and consequently, all higher-order derivatives are equally discontin- 
uous at those points. In practice, these discontinuities are smoothed out by 
the inertia of the links and the motors, if the SP are chosen close enough. 
Obviously, higher-order continuity can be achieved if higher-order splines, 
e.g., quintic splines, are used instead. For the sake of conciseness, these 
splines are not discussed here, the interested reader being directed to the 
specialized literature (Dierckx, 1993). 

Further, the fV x 3 matrices P and P" are defined as 





T ^ 

Pi 




r(p'/ri 


p = 


T 

P2 


, P"^ 






1 

. 

a 

1 







which allows us to rewrite eqs. (9.49b) in matrix form as 



AP" = 6CP 



(9.56) 



It is now apparent that the spline coefficients aj,, . . . , dfc can be calculated 
once vectors p(( are available. These vectors can be computed via matrix 
P" as the solution to eq.(9.56). However, finding this solution requires 
inverting the {N — 2)xN matrix A, which is rectangular and hence cannot 
be inverted, properly speaking. We thus have an underdetermined system 
of linear equations, and further conditions are needed in order to render it 
determined. Such conditions are those defining the type of spline at hand. 
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For example, closed paths call naturally for periodic splines, while open 
paths call for other types such as natural splines. The conditions imposed 
on periodic parametric splines are listed below: 

Pw = Pi, Pw = Pi, Pw = Pi (9.57a) 



On the other hand, natural parametric splines are obtained under the 
conditions 

Pi = Pw = 0 (9.57b) 



Thus, if a periodic parametric spline is required, then vectors pn and p'^ 
can be deleted from matrices P and P", respectively, these then becoming 
{N — 1) X 3 matrices, namely. 





T ^ 

Pi 




r iPfV 1 


p = 


T 

P2 


, p"^ 


(P^O^ 




1 

t) 

1 




1 

• 

1 



(9.58) 



Moreover, the first-derivative condition of eq. (9.57a) is added to the N — 2 
continuity conditions of eq.(5.56), thereby obtaining N — 1 equations of this 
form. Consequently, A becomes an {N — 1) x {N — 1) matrix. Correspond- 
ingly, C also becomes an {N — 1) x {N — 1) matrix, i.e.. 



2aiN' Oil 0 
Ol 2 q:i_2 02 

0 02 202,3 



0 

0 

03 



ON' 

0 

0 



(9.59a) 



and 



C = 



0 

ON' 



Pl,N' 

fh 

0 



0 

0 



aN" 



2aN',"N" 

UN" 



O.N" 

2on,"N' -I 



/3i 0 0 

■/3i,2 fh 0 

h ~h,3 h 



pN' 

0 

0 



(9.59b) 



0 0 

- Pn' 0 



0 



pN‘ 



■pN',"N" pN" 
pN" —pN,"N' - 



Since A is nonsingular, eq.(9.56) can be solved for P", namely, 

P" = 6A-^CP (9.60) 



thereby computing all vectors {pf}i-\ from which p'f^ can be readily 
obtained. Hence, the spline coefficients follow. 

Likewise, if natural parametric splines are used, then P" becomes an 
{N — 2) X 3 matrix, while A, consequently, becomes an {N — 2) x {N — 2) 
matrix, as given in eq.(5.59). 
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TABLE 9.2. The Cartesian coordinates of the supporting points 



ra 


0 ° 


CO 

O 

o 


60 ° 


o 

o 


120 ° 


150 ° 




0.45 


0.429904 


0.375 


0.3 


0.225 


0.170096 


n 


0 


0.075 


0.129904 


0.15 


0.129904 


0.075 


D 


0.396863 


0.411774 


0.45 


0.497494 


0.540833 


0.570475 


ra 


180 ° 


210 ° 


240 ° 


270 ° 


300 ° 


330 ° 




0.15 


0.170096 


0.225 


0.3 


0.375 


0.429904 


n 


0 


- 0.075 


- 0.129904 


- 0.15 


- 0.129904 


- 0.075 


D 


0.580948 


0.570475 


0.540833 


0.497494 


0.45 


0.411774 
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Example 9.4.1 (Spline-approximation of a warped path) For the 

numerical values R = 0.6 m, r = 0.15 m, and d = 0.3 m, determine 
the periodic parametric cubic spline approximating the intersection of the 
sphere and the cylinder of Fig. 9.f, with 12 equally spaced supporting points 
along the cylindrical coordinate p, i.e., with supporting points distributed 
along the intersection curve at intervals Ap = 30°. Using the spline, find 
values of the tangent, normal, and binomial vectors of the curve, as well as 
the rotation matrix Q. In order to quantify the error in this approximation, 
compare (i) the components of the two position vectors, the exact and the 
spline- generated ones, while normalizing their differences using the radius 
of the cylinder r; and (ii) the Euler-Rodrigues parameters of the exact and 
the spline-approximated rotation matrices. Plot these errors vs. ip. 

Solution: We use eq.(9.37) to find the Cartesian coordinates of the sup- 
porting points. The numerical results are given in terms of the components 
of r = \x, y, zY in Table 9.2. Note that this table does not include the 
Cartesian-coordinate values at 360° because these are identical with those 
at 0°. 

The four Euler-Rodrigues parameters {c}f^g of the rotation matrix are 
most suitably calculated in terms of the linear invariants, i.e., as appearing 
in eq.(2.77). If we let p and r denote the estimates of p and r, respectively, 
then the orientation error is evaluated via the the four differences Arq = 
rq — Vi, for * = 0, ... , 3. The positioning error is computed, in turn, as the 
normalized difference e = (p — l5)/r to yield a dimensionless number, its 
components being denoted by Cj,, ty, and e^. The components of the two 
errors are plotted vs. in Figs. 9.5 and 9.6. Note that the orientation errors 
are, roughly, one order of magnitude greater than the positioning errors. 



9.5 Continuous-Path Tracking 

When a continuous trajectory is to be tracked with a robot, the joint angles 
have to be calculated along a continuous set of poses of the end-effector. 
In practice, the continuous trajectory is sampled at a discrete set of close- 
enough poses { Sfc along the continuous trajectory. Then in principle, 
an IKP must be solved at each sampled pose. If the manipulator is of the 
decoupled type, these calculations are feasible in a fraction of a millisecond, 
for the solution reduces, in the majority of the cases, to a cascading of 
quadratic equations. In the worst case, the inverse kinematics of a decoupled 
manipulator requires finding all the roots of a quartic equation at each 
sampled pose, but this is still feasible in the same time frame, for the 
four roots of interest can be calculated from formulas. However, if the 
manipulator has an architecture not lending itself to a simple solution and 
requires solving polynomials of a degree higher than four, then finding all 
solutions at each sample pose may require a few milliseconds, which may 
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be too slow in fast operations. Hence, an alternative approach is needed. 

The alternative is to solve the IKP iteratively. That is, if we have the 
value of the vector of joint variables 6{tk) and want to find its value at 
tfc+i, then we use Algorithm 9.5.1. 

Various procedures are available to find the correction A0 of Algo- 
rithm 9.5. The one we have found very convenient is based on the Newton- 
Gauss method (Dahlquist and Bjorck, 1974). In the realm of Newton meth- 
ods — there are several of these, the Newton-Gauss and the Newton- Raphson 
methods being two of this class — the closure equations (4.9a & b) are writ- 
ten in the form 

i{6)=sa (9.61) 

where is the 7-dimensional prescribed-pose array. We recall here the def- 
inition of the pose array introduced in Section 3.2 to represent s^, namely. 



Sd = 



q 

qo 



(9.62) 



LPJd 

with q and defined, in turn, as a 3-dimensional vector invariant of the 
rotation Q and its corresponding scalar, respectively. Moreover, p is the 
position vector of the operation point. Therefore, the 7-dimensional vector 
f is defined, correspondingly, as 





'W)' 




q 


f(0)^ 


fo(O) 


= 


qo 








.p. 



(9.63) 



where denotes the counterpart of q above, as pertaining to the prod- 
uct Qi Qe of eq.(4.9a); fo{0) is the counterpart of (/o, as pertaining to 
the same product; and fp(0) is the sum ai + • • • + Qi • • • Qsae. In principle, 
any of the three types of rotation invariants introduced in Section 3.2 can 
be used in the above formulation. 

Now, eq.(9.61) represents a nonlinear system of seven equations in six 
unknowns. The system is thus overdetermined, but since the four rotational 
equations are consistent, this system should admit an exact solution, even 
if this solution is complex. For example, if p is specified in Sd above as lying 
outside of the manipulator reach, then no real solution is possible, and the 
solution reported by any iterative procedure capable of handling complex 
solutions will be complex. 

Upon application of the Newton-Gauss method to find a solution of 
eq.(9.61), we assume that we have an initial guess 0°, and based on this 
value, we generate a sequence 0^, . . ., 0®, 0®^^, . . ., until either a conver- 
gence or an abortion criterion is met. This sequence is generated in the 
form 

Qi+I ^ (9 64) 

with A0® calculated from 



$(6>®)A6>® = - 



f(0*)+Sd 



(9.65) 
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and $ defined as the Jacobian matrix of f( 0 ) with respect to 6 . Note that 
by virtue of its definition, $ is a 7 x 6 matrix. A common misconception in 
the robotics literature is to confuse this Jacobian matrix with the Jacobian 
defined by Whitney (1972) and introduced in eq.(4.54a), which maps joint 
rates into the EE twist. The difference between the two Jacobians being 
essential, it is made clear in the discussion below. First and foremost, $ is 
an actual Jacobian matrix, while Whitney’s Jacobian, properly speaking, 
is not. In fact, $ is defined as 




(9.66) 



In order to find $ in eq.(9.65), we note that by application of the chain 
rule. 



f 






(9.67) 



However, from the definition of f, we have that f is the time-derivative 
of the pose array of the EE, i.e., s. Moreover, by virtue of eq.(3.79), this 
time-derivative can be expressed as a linear transformation of the twist t 
of the EE, i.e., 

f = Tt (9.68a) 

with T defined in Section 3.2 as 



T = 



F O43 

O33 I33 



(9.68b) 



where O33 and O43 denote the 3x3 and the 4x3 zero matrices, I33 
being the 3x3 identity matrix. Further, matrix F takes on various forms, 
depending on the t}q)e of rotation representation adopted, as discussed in 
Section 3.2. 

We write next the left-hand side of eq.(9.68a) as shown in eq.(9.67), and 
the twist t of the right-hand side of eq.(9.68a) in terms of 0 , as expressed 
in eq.(4.53), thereby obtaining 



$6> = TJ6> (9.69) 

which is a relation valid for any value of 0. As a consequence, then, 

$ = TJ (9.70) 

whence the relation between the two Jacobians is apparent. Note that 
eq.(9.68a) allows us to write 



f = TJ6> (9.71) 

Upon equating the right-hand sides of eqs.(9.71) and (9.68a), we obtain 

TJ 0 = Tt = Sd (9.72) 
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If linear invariants are used to represent the rotation, then T becomes 
rank-deficient if and only if the angle of the rotation becomes tt (Tandirci 
et al., 1994); otherwise, T is always of full rank, and eq.(9.72) leads to 

J0 = t (9.73) 

which is exactly the same as eq.(4.53). Now we multiply both sides of the 
foregoing equation by At, thereby obtaining 

JA6> = tAt (9.74) 

All we need now is, apparently, the product in the right-hand side of the 
above equation, namely. 



tAt 



u)At 




u)At 


pAt 




. ^P. 



(9.75) 



The product tuAt is found below, in terms of the orientation data available: 
First and foremost, it is common practice in the realm of Newton methods 
to assume that a good enough approximation to the root sought is available, 
and hence, A9 is “small.” That is, we assume that ||A0|| is small, where 
II • II denotes any vector norm. Moreover, we use the end-effector pose at 
t = tfc as a reference to describe the desired pose at t = tfc+i, the rotation 
sought — that takes the EE to its desired attitude — being denoted by AQ, 
and defined as (AQ)Qfc = Qd, when all rotations are expressed in the same 
frame and Qj, represents the orientation of the EE ai t = tk- Thus, 



AQ QdQfc 



(9.76) 



Now we relate ioAt with AQ. To this end, notice that 



u)At = vect(riAt) (9.77a) 

with denoting the cross-product matrix of u). On the other hand, AQ is 
bound to be a rotation about an axis parallel to a unit vector e, through a 
small angle A<f>, and hence, from eq.(2.48), 

AQ « 1 + {A(f))E (9.77b) 



where E is the cross-product matrix of e. It is then possible to assume that 
riAt, as appearing in eq.(9.77a), is the skew-symmetric component of AQ, 
as given by eq.(9.77b), i.e.. 



AQ = 1 + nAt 

whence 

riAt = QdQl - 1 
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which readily leads to 

wAt = vect(QdQ^) 

thereby obtaining the relation sought. 

In summary, then, the correction A9 is computed from 

JA6> = At 



(9.78) 

(9.79) 



= r vect(QdQD 



with At defined as 



and Ap defined, in turn, as the difference between the prescribed value 
of the position vector of the operation point and its value pk at the current 
iteration. Thus, the numerical path-tracking scheme consists essentially of 
eqs.(9.79) and (9.80), as first proposed by Pieper (1968). We thus have 
Algorithm 9.5.2. 



When implementing the foregoing procedure, we want to save processing 
time; hence, we aim at fast computations. The computation of the cor- 
rection A9 involves only linear-equation solving, which was discussed at 
length in Chapter 4 and need not be discussed further here. The only item 
that still needs some discussion is the calculation of the vector norm ||A0||. 
Since any norm can be used here, we can choose the norm that is fastest to 
compute, namely, the maximum norm, also known as the Chebyshev norm, 
represented as ||A0||oo, and defined as 



||A6>||oo = max{ |6»j| } 

i 



(9.81) 
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Note that this norm only requires comparisons and no floating-point oper- 
ations. The Euclidean norm of an n-dimensional vector, however, requires 
n multiplications, n — 1 additions, and one square root. 

Example 9.5.1 (Path-tracking for arc-welding) We want to weld the 
sphere and the cylinder of Example 9.3.2 using a robot for arc welding, 
e.g., a Fanuc Arc Mate, whose Denavit-Hartenberg parameters are listed in 
Table f.2. Furthermore, the welding seam to be tracked is placed well within 
the workspace of the manipulator. A location found quite .suitable for this 
task was obtained with the aid of RVS, our Robot Visualization System. 
This location requires that the coordinate frame Tc of Fig. 9.f have its 
axes parallel pairwise to those of the robot base, T\. The latter is defined 
according to the Denavit-Hartenberg notation, and so Z\ coincides with the 
axis of the first revolute; it is, moreover, directed upwards. The position 
found for the origin Oc of tFc, of position vector o, is given in T\ as 

X —1.0 

[o]i = y = —0.1 m 

[z\ [ 0.5 

Find the time-histories of all the joint variables that will perform the desired 
operation with the tip of the electrode traversing the intersection curve at 
the constant speed of vq = 0.1 m/s. Furthermore, plot the variation of the 
condition number of the .Jacobian matrix along the path. 

Solution: The robot at hand was studied in Subsection 8.2.6, where it was 
found not to be of the decoupled type. In fact, this robot does not admit a 
closed-form inverse kinematics solution, and hence, the foregoing iterative 
procedure is to be used. 

At the outset, we calculate all inverse kinematics solutions at the pose 
corresponding to = 0 using the bivariate-equation approach of Subsec- 
tion 8.2.2. This pose is defined by the orthogonal matrix Q and the position 
vector p given below: 

'0.6030 0 -0.7977] f -0.5500' 

[Q]i = [e6 et e„]= 0 1 0 , [p]i = -0.100 m 

0.7977 0 0.6030 J [ 0.8969 

with both Q and p given in robot-base coordinates. The contours for the 
above pose, which were obtained using the procedure of Subsection 8.2.2, 
are shown in Fig. 9.7, the eight solutions obtained being summarized in 
Table 9.3, which includes the condition number of the Jacobian, k(J), of 
each solution. Note that the calculation of k(J) required computing the 
characteristic length of the robot, as explained in Section 4.9. This length, 
as calculated in that section, turned out to be L = 0.3573 m. 

Now, we have eight solutions at our disposal, from which we must choose 
one for path-tracking. In the absence of any criterion to single out one 
specific solution, we can pick up the solution with the lowest condition 
number. If we do this, we end up with solution 1 in Table 9.3. However, 
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6»4 (deg) 



FIGURE 9.7. Contour solutions of the Fanuc Arc Mate robot at the given EE 
pose. 



TABLE 9.3. inverse kinematics solutions of the Eanuc Arc Mate robot for the 
given EE pose. 
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«(J) 




62 


6 s 


f 


4.74 


19.9039° 


124.909° 


-176.484° 


2 


4.85 


-3.6664° 


124.723° 


-173.071° 


3 


11.12 


-154.951° 


-67.5689° 


-135.549° 


4 


6.31 


-176.328° 


-63.4487° 


-129.817° 


5 


4.79 


-176.341° 


75.1632° 


-76.6692° 


6 


5.20 


-153.567° 


73.4546° 


-72.5407° 


7 


8.68 


-3.6362° 


-129.644° 


-32.9672° 


8 


9.94 


18.9031° 


-131.096° 


-26.8084° 



i 


64 


O 5 


6 e 


1 


16.1379° 


-102.29° 


-15.8409° 


2 


177.019° 


101.19° 


-177.208° 


3 


141.716° 


146.966° 


17.754° 


4 


-4.5893° 


-140.319° 


-178.681° 


5 


3.7343° 


51.4104° 


-179.877° 


6 


-153.868° 


-53.7328° 


-0.5046° 


7 


-175.011° 


-144.428° 


178.133° 


8 


-28.6793° 


147.417° 


13.0786° 
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when we attempted to track the given path with this solution, it turned out 
that this solution encountered a singularity and was hence discarded. Of 
the seven remaining solutions, solution 5 has the lowest condition number; 
this solution led to a singularity- free trajectory. 

Once the appropriate solution is chosen, the trajectory can be tracked 
with the aid of Algorithm 9.5.2. Here, we need a discrete set of poses at 
equal time- intervals. Note that we can produce such a set at equal intervals 
of angle because we have expressions for the pose variables in terms of 
this angle. In order to obtain this set at equal time-intervals, then, we need 
angle as a function of time, i.e., y(t). In the sequel, we will also need the 
time T needed to complete the task. Now, since the speed of the electrode 
tip is constant and equal to vq, the time T is readily obtained by dividing 
the total length I of the curve by vq- The length of the curve, in turn, can 
be computed as s( 27 t), where function s(y>) denotes the arc length as a 
function of angle y>, i.e.. 



s ( a ) = / \Wi^)\\dif 

Jo 

We thus obtain, by numerical quadrature, 

I = s( 27 t) = 1.0257 m 

Hence, the total time is 

T = — = 10.257 s 

Vo 



(9.82) 



, _ ds ds d(f . ds 

dt dip dt ^ dip 



Now, in order to obtain y(t), we first calculate s as 

(9.83a) 

Furthermore, we note that ds/dip = ||r'((p)||, which allows us to write s 

s = 9^||r'(^)|| 

Moreover, ||r'((p)|| was found in eq.(9.39b) to be 

||r'(^)||=rG(^) 

s thus becoming 



as 



s = rGip 



(9.83b) 



Furthermore, we recall the expression derived for G'(y) in eq.(9.39e). This 
expression, along with the constancy condition on s, i.e., s = vq, leads to 



npy/l + (A(psiny>)2 = 



Vo 
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where r is the radius of the cylinder. Upon solving for from the above 
equation, we obtain 



— 2A cos If 

— 2A cos + A^ sin^ f 

which is a nonlinear first-order differential equation for y(t). Its initial value 
can be assigned as y>(0) = 0, thereby formulating a nonlinear first-order 
initial- value problem. The numerical solution of the foregoing problem is 
nowadays routine work, which can be handled with suitable software, e.g., 
Matlab (Hanselman and Littlefied, 2001). Upon solving this equation, a 
data file is produced that contains the time-history of y>. The plot of vs. 
nondimensional time is displayed in Fig. 9.8a. Since the variations of f(t) 
are relatively small, this plot provides little information on the time- history 
of interest. A more informative plot, that of (p(t), is included in Fig. 9.8b 
for this reason. Apparently, turns out to be the sum of a linear and a 
periodic term. 

With f(t) known as a function of time, we can now specify the pose of 
the end-effector, i.e., p and Q, as functions of time. 

The whole trajectory was tracked with the robot at hand using the algo- 
rithm outlined in this section. With the aid of this algorithm, we produced 
the plots of Fig. 9.9. Also, the time-history of the condition number of the 
manipulator Jacobian was computed and plotted in Fig. 9.10. Apparently, 
the condition number of the Jacobian remains within the same order of 
magnitude throughout the whole operation, below 10, thereby showing that 
the manipulator remains far enough from singularities during this task — 
the condition number becomes very large when a singularity is approached, 
becoming unbounded at singularities. A rendering of the welding seam with 
the Ffenet-Serret triad at a sample of points is displayed in Fig. 9.11. It is 
noteworthy that the torsion of the path is manifested in this figure by virtue 
of the inclination of the Z axis, which changes from point to point. In a 
planar curve, this axis would remain at a fixed orientation while traversing 
the curve. 
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(a) 




(b) 

FIGURE 9.8. Plot of ip vs. nondimensional time. 





FIGURE 9.9. Time-histories of the joint variables (in degrees) of the Fanuc Arc 
Mate robot used to track a warped curve for arc-welding vs. nondimensional time. 
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FIGURE 9.10. Time-history of the condition number of the Jacobian matrix 
during an arc-welding operation vs. nondimensional time. 




FIGURE 9.11. Welding seam with Frenet-Serret frames. 




10 

Dynamics of Complex Robotic 
Mechanical Systems 



10.1 Introduction 

The subject of this chapter is the dynamics of the class of robotic mechani- 
cal systems introduced in Chapter 8 under the generic name of complex. No- 
tice that this class comprises serial manipulators not allowing a decoupling 
of the orientation from the positioning tasks. For purposes of dynamics, 
this decoupling is irrelevant and hence, was not a condition in the study of 
the dynamics of serial manipulators in Chapter 6. Thus, serial manipulators 
need not be further studied here, the focus being on parallel manipulators 
and rolling robots. The dynamics of walking machines and multifingered 
hands involves special features that render these systems more elaborate 
from the dynamics viewpoint, for they exhibit a time-varying topology. 
What this means is that these systems include kinematic loops that open 
when a leg takes off or when a finger releases an object and open chains that 
close when a leg touches ground or when a finger makes contact with an 
object. The implication here is that the degree of freedom of these systems 
is time- varying. The derivation of such a mathematical model is discussed 
in (Pfeiffer et al, 1995). 

The degree of freedom (dof) of the mechanical systems studied here is 
thus constant. Now, the two kinds of systems studied here pertain to very 
different types, for parallel manipulators fall into the realm of holonomic, 
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while rolling robots into that of nonholonornic, mechanical systems. In or- 
der to better understand this essential difference between these two types 
of systems, we give below a summary of the classification of mechanical 
systems at large. 



10.2 Classification of Robotic Mechanical Systems 
with Regard to Dynamics 

Because robotic mechanical systems are a class of general mechanical sys- 
tems, a classification of the latter will help us focus on the systems moti- 
vating this study. Mechanical systems can be classified according to vari- 
ous criteria, the most common one being based on the type of constraints 
to which these systems are subjected. In this context we find holonomic 
vs. nonholonornic and scleronornic vs. rheonomic constraints. Holonomic 
constraints are those that are expressed either as a system of algebraic 
equations in displacement variables, whether angular or translational, not 
involving any velocity variables, or as a system of equations in velocity vari- 
ables that nevertheless can be integrated as a whole to produce a system 
of equations of the first type. Note that it is not necessary that every sin- 
gle scalar equation of velocity constraints be integrable; rather, the whole 
system must be integrable for the system of velocity constraints to lead to 
a system of displacement constraints. If the system of velocity constraints 
is not integrable, the constraints are said to be nonholonornic. Moreover, 
if a mechanical system is subject only to holonomic constraints, it is said 
to be holonomic; otherwise, it is nonholonornic. Manipulators composed 
of revolute and prismatic pairs are examples of holonomic systems, while 
wheeled robots are usually nonholonornic systems. On the other hand, if a 
mechanical system is subject to constraints that are not explicit functions 
of time, these constraints are termed scleronornic, while if the constraints 
are explicit functions of time, they are termed rheonomic. For our purposes, 
however, this distinction is irrelevant. 

In order to understand better one more classification of mechanical sys- 
tems, we recall the concepts of generalized coordinate and generalized speed 
that were introduced in Subsection 6.3.2. The generalized coordinates of a 
mechanical system are all those displacement variables, whether rotational 
or translational, that determine uniquely a configuration of the system. 
Note that the set of generalized coordinates of a system is not unique. 
Moreover, various sets of generalized coordinates of a mechanical system 
need not have the same number of elements, but there is a minimum num- 
ber below which the set of generalized coordinates cannot define the con- 
figuration of the system. This minimum number corresponds, in the case of 
holonomic systems, to the degree of freedom of the system. Serial and paral- 
lel manipulators coupled only by revolute or prismatic pairs are holonomic. 
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their joint variables, grouped in vector 9 , playing the role of generalized 
coordinates, while their joint rates, grouped in vector 9 , in turn, play the 
role of generalized speeds. Note that in the case of parallel manipulators, 
not all joint variables are independent generalized coordinates. In the case 
of nonholonomic systems, on the other hand, the number of generalized co- 
ordinates needed to fully specify their configuration exceeds their degree of 
freedom by virtue of the lack of integrability of their kinematic constraints. 
This concept is best illustrated with the aid of examples, which are included 
in Section 10.5. Time-derivatives of the generalized coordinates, or linear 
combinations thereof, are termed the generalized speeds of the system. If 
the kinetic energy of a mechanical system is zero when all its generalized 
speeds are set equal to zero, the system is said to be catastatic. If, on the 
contrary, the kinetic energy of the system is nonzero even if all the gener- 
alized speeds are set equal to zero, the system is said to be acatastatic. All 
the systems that we will study in this chapter are catastatic. A light robot 
mounted on a heavy noninertial base that undergoes a controlled motion is 
an example of an acatastatic system, for the motion of the base can be as- 
sumed to be insensitive to the dynamics of the robot; however, the motion 
of the base does affect the dynamics of the robot. 

Another criterion used in classifying mechanical systems, which pertains 
specifically to robotic mechanical systems, is based on the type of actua- 
tion. In general, a system needs at least as many independent actuators 
as degrees of freedom. However, instances arise in which the number of 
actuators is greater than the degree of freedom of the system. In these 
instances, we speak of redundantly actuated systems. In view of the funda- 
mental character of this book, we will not study redundant actuation here; 
we will thus assume that the number of independent actuators equals the 
degree of freedom of the system. 

The main results of this chapter are applicable to robotic mechanical 
systems at large. For brevity, we will frequently refer to the objects of our 
study simply as systems. 



10.3 The Structure of the Dynamics Models of 
Holonomic Systems 

We saw in Section 6.6 that the mathematical model of a manipulator of the 
serial type contains basically three terms, namely, one linear in the joint 
accelerations, one quadratic in the joint rates, and one arising from the 
environment, i.e., from actuators, dissipation, and potential fields such as 
gravity. We show in this section that in fact, the essential structure of this 
model still holds in the case of more general mechanical systems subject 
to holonomic constraints, if we regard the rates of the actuated joints as 
the independent generalized speeds of the system. Nonholonomic robotic 
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systems are studied in Section 10.5. 

First, we will assume that the mechanical system at hand is composed of 
r rigid bodies and its degree of freedom is n. Henceforth, we assume that 
these bodies are coupled in such a way that they may form kinematic loops; 
for this reason, such systems contain some unactuated joints. Definitions 
similar to those of Section 6.3.1 are henceforth adopted. In this vein, the 
manipulator mass matrix of that section becomes now, more generically, the 
6r X 6r system mass matrix M, the 6r x 6r system angular velocity matrix 
W, and the 6r-dimensional system twist vector t being defined likewise. 

We assume further that the total number of joints, active and passive, 
is m > n. The m-dimensional array 6 of joint variables, associated with 
both actuated and unactuated joints, is thus naturally partitioned into two 
subarrays, the n-dimensional vector of actuated joint variables 6a and its 
m'-dimensional unactuated counterpart 0„, with m' = m — n, namely. 



0 



( 10 . 1 ) 



We can now set up the mathematical model of the system at hand using 
the natural orthogonal complement, as introduced in Section 6.5. Since the 
system under study has n degrees of freedom, the model sought must be a 
system of n second-order ordinary differential equations. We can proceed to 
derive this model as we did in Section 6.5, by regarding all joints first as if 
they were independent, but taking into account that only n of the total m 
joints are actuated. We do this by introducing a vector of constraint forces, 
as is done in the realm of Lagrangian dynamics (Torok, 2000). In this vein, 
we first represent the twists of all the moving links as linear transformations 
of the joint-rate vector 6, then assemble all the individual 6-dimensional 
twist arrays into the 6r-dimensional array t defined above as the system 
twist. We thus end up with a relation of the form 



t = U(6>)6> (10.2) 

where U(0) is the 6r x m twist-shaping matrix, playing a role similar to 
that of matrix T of Section 6.5. Moreover, the constraints relating all joint 
rates can be cast in the form 



A(6>)6> = Op (10.3) 

where A{9) is a p x m matrix, whereby p < m, with nullity — the nullity of a 
matrix is the dimension of its nullspace — v = n, and Op is the p-dimensional 
zero vector. Given the nullity of A{9), up to n of the m components of 9 
can be assigned freely without violating the constraints (10.3), which is 
compatible with the assumption on the dof of the system. Note that, in 
setting up the foregoing p constraints on the joint rates, the number p 
depends on the topology of the system, i.e., on its number of links; on its 
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number of joints; and on how the links are coupled, so as to form kinematic 
loops. 

In applying the procedure of the natural orthogonal complement to the 
constrained system, we end up with a system of m second-order ordinary 
differential equations, namely, the Euler-Lagrange equations of a system 
constrained by the relations (10.3), which thus takes the form 

W + C(6>, 0)0 = f + 5 + 7 + + A^A (10.4a) 

The above equation contains terms that are familiar from Section 6.5, 
except for the last term of the right-hand side. This term accounts for 
the generically termed constraint forces and amounting to constraint joint 
torques and forces that must be exerted at all joints in order to main- 
tain the topology of the system. Vector A is termed the vector of Lagrange 
multipliers in the realm of Lagrangian dynamics. In the above equation, 
the definitions below, similar to those of eqs.(6.58) and (6.59), have been 
introduced: 



1(0) = U^MU (10.4b) 

C(0,0) = U^MU + U^WMU (10.4c) 

f = U^w"^, 5 = U^w^, 7 = U^w^ (10.4d) 

Moreover, w'^, w®, w^, and are the various types of wrenches acting 
on the system: exerted by the actuators; stemming from dissipation effects; 
due to the gravity field; and exerted by the environment, respectively. In 
turn, J is the 6 x m Jacobian matrix mapping the system joint rates into 
the end-effector twist, while is assumed applied onto the end-effector. 

Upon resorting to the kinematics of the system, it is possible to express 
the vector of joint rates 0 as a linear transformation of the vector of actu- 
ated joint rates 0a, namely. 



0 = 0(0a)0a (10.5) 

where we have assumed that, from the geometry of the system, 0 „ has 
been solved for in terms of 0„. Further, upon substitution of eq.(10.5) into 
eq.(10.3)), we obtain 

A(0)0(0a)0a = Op 

which must hold for any 9a, given the dof of the system. As a consequence, 
then, 

A(0)0(0a) = Opm (10.6) 

and hence, 0(0a) is an orthogonal complement of A(0), which we can also 
call a natural orthogonal complement. Notice, however, that contrary to 
the natural orthogonal complement U, which maps the joint-rate vector 
onto the system twist, 0 maps the space of actuated joint rates into that 
of the system joint rates. Apparently, 

0=0(0a)0a + 0(0a,0a)0a 



(10.7) 
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Upon substitution of eq.(10.7) into eq. (10.4a), we obtain 

i&0a + i&0a + C{0a,0a)&Oa =f + 5 + J + 3^ + A^A 

Further, the term of constraint forces is eliminated from the above equa- 
tions upon multiplying both sides of the above equation by 0^ from the 
left, thus obtaining the mathematical model sought, i.e., 

10a + C0a=T + d + 'r + 3^w^ (10.8a) 

with the definitions below: 

I = T^MT, C = T^MT + T^WMT, J = J0, (10.8b) 

T = 0^T, d = Q^d, 7 = 0^7, (10.8c) 

and 

T = U0 (10. 8d) 

That is, the mathematical model governing the dynamics of any holonomic 
robotic mechanical system is formally identical to that of eq.(6.61) obtained 
for serial manipulators. 



10.4 Dynamics of Parallel Manipulators 

We illustrate the modeling techniques of mechanical systems with kine- 
matic loops via a class of systems known as parallel manipulators. While 
parallel manipulators can take on a large variety of forms, we focus here on 
those termed platform manipulators, with an architecture similar to that of 
flight simulators. In platform manipulators we can distinguish two special 
links, namely, the base B and the moving platform A4. Moreover, these 
two links are coupled via six legs, with each leg constituting a six-axis kine- 
matic chain of the serial type, as shown in Fig. 10. 1, whereby a wrench 
w^, represented by a double- headed arrow, acts on A4 and is applied at 
the mass center of A4. This figure shows the axes of the revolutes cou- 
pling the legs to the two platforms as forming regular polygons. However, 
the modeling discussed below is not restricted to this particular geometry. 
As a matter of fact, these axes need not even be coplanar. On the other 
hand, the architecture of Fig. 10. 1 is very general, for it includes more spe- 
cific types of platform manipulators, such as flight simulators. In these, the 
first three revolute axes stemming from the base platform have intersecting 
axes, thereby giving rise to a spherical kinematic pair, while the upper two 
axes intersect at right angles, thus constituting a universal joint. Moreover, 
the intermediate joint in flight simulators is not a revolute, but rather a 
prismatic pair, which is the actuated joint of the leg. A leg kinematically 
equivalent to that of flight simulators can be obtained from that of the 
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FIGURE 10.1. A platform-type parallel manipulator. 

manipulator of Fig. 10.1, if the intermediate revolute has an axis perpen- 
dicular to the line connecting the centers of the spherical and the universal 
joints of the corresponding leg, as shown in Fig. 10.2. In flight simulators, 
the pose of the moving platform is controlled by hydraulic actuators that 
vary the distance between these two centers. In the revolute-coupled equiv- 
alent leg, the length of the same line is controlled by the rotation of the 
intermediate revolute. 

Shown in Fig. 10.3 is the graph of the system depicted in Fig. 10.1. In 
that graph, the nodes denote rigid links, while the edges denote joints. By 
application of Euler’s formula for graphs (Harary, 1972), the number i of 
independent loops of a system with many kinematic loops is given by 

i = j-l + l (10.9) 

where j is the number of revolute and prismatic joints and I is the number 
of links. 

Thus, if we apply Euler’s formula to the system of Fig. 10.1, we conclude 
that its kinematic chain contains five independent loops. Hence, while the 
chain apparently contains six distinct loops, only five of these are indepen- 
dent. Moreover, the degree of freedom of the manipulator is six. Indeed, 
the total number of links of the manipulator is/ = 6x5 + 2 = 32. Of these, 
one is fixed, and hence, we have 31 moving links, each with six degrees of 
freedom prior to coupling. Thus, we have a total of 31 x 6 = 186 degrees 
of freedom at our disposal. Upon coupling, each revolute removes five de- 
grees of freedom, and hence, the 36 kinematic pairs remove 180 degrees of 
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FIGURE 10.2. A leg of a simple platform-type parallel manipulator. 

freedom, the manipulator thus being left with 6 degrees of freedom. We 
derive below the mathematical model governing the motion of the overall 
system in terms of the independent generalized coordinates associated with 
the actuated joints of the legs. 

We assume, henceforth, that each leg is a six-axis open kinematic chain 
with either revolute or prismatic pairs, only one of which is actuated, and 
we thus have as many actuated joints as degrees of freedom. Furthermore, 
we label the legs with Roman numerals I, II, . . ., VI and denote the mass 
center of the mobile platform A4 by , with the twist of A4 denoted by 
ty 4 and defined at the mass center. That is, if denotes the position 
vector of in an inertial frame and cy^ its velocity, while tuy^ is the 
angular velocity of A4, then 

tM= ( 10 . 10 ) 

L _ 

Next, the Newton-Euler equations of A4 are derived from the free-body 
diagram shown in Fig. 10.4. In this figure, the legs have been replaced by the 
constraint wrenches acting at point the governing equation 

thus taking the form of eq.(6.5c), namely, 

VI 

M^M^M + + X] Wj (10.11) 

J=I 

with denoting the external wrench acting on A4. Furthermore, let us 
denote by qj the variable of the actuated joint of the Jth leg, all variables 
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FIGURE 10.3. The graph of the flight simulator. 




FIGURE 10.4. The free-body diagram of M. 
of the six actuated joints being grouped in the 6-dimensional array q, i.e., 

q=['?/ '?// ••• (10.12) 

Now, we derive a relation between the twist and the active joint rates, 
(/j, for J = /, //, . . ., VT. To this end, we resort to Fig. 10.5, depicting the 
Jth leg as a serial-type, six-axis manipulator, whose twist-shape relations 
are readily expressed as in eq. (4.54a), namely, 

Jj0j = ty4, J = U/ (10.13) 

where Jj is the 6x6 Jacobian matrix of the Jth leg. 

In Fig. 10.5, the moving platform A4 has been replaced by the constraint 
wrench transmitted by the moving platform onto the end link of the Jth 
leg, — Wj , whose sign is the opposite of that transmitted by this leg onto A4 
by virtue of Newton’s third law. The dynamics model of the manipulator 
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FIGURE 10.5. The serial manipulator of the Jth leg. 



of Fig. 10.5 then takes the form 

ijej + Cj{ej,ej)9j = Tj j = i,ii, ...,vi (io.i4) 



where Ij is the 6x6 inertia matrix of the manipulator, while Cj is the 
matrix coefficient of the inertia terms that are quadratic in the joint rates. 
Moreover, 6j and tj denote the 6-dimensional vectors of joint variables 
and joint torques, namely. 




(10.15) 



with subscript Jk denoting in turn the only actuated joint of the Jth leg, 
namely, the A:th joint of the leg. If we now introduce ej^, defined as a unit 
vector all of whose entries are zero except for the A:th entry, which is unity, 
then we can write 

Tj = fjejk (10.16) 

If the actuated joint is prismatic, as is the case in flight simulators, then 
fj is a force; if this joint is a revolute, then fj is a torque. 
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Now, since the dimension of q coincides with the degree of freedom of 
the manipulator, it is possible to find, within the framework of the natural 
orthogonal complement, a 6 x 6 matrix Lj mapping the vector of actuated 
joint rates q into the vector of Jth-leg joint-rates, namely, 

0j = Ljq, J = 1// (10.17) 

The calculation of Lj will be illustrated with an example. 

Moreover, if the manipulator of Fig. 10.5 is not at a singular configura- 
tion, then we can solve for Wj from eq.(10.14), i.e., 

Wj = — Ij0j — Cj0j) (10.18) 

in which the superscript —T stands for the transpose of the inverse, or 
equivalently, the inverse of the transpose, while Ij = Ij{9j) and Cj = 
Cj{9j, 9j). Further, we substitute Wj as given by eq.(10.18) into eq.(lO.ll), 
thereby obtaining the Newton-Euler equations of the moving platform free 
of constraint wrenches. Additionally, the equations thus resulting now con- 
tain inertia terms and joint torques pertaining to the Jth leg, namely, 

VI 

Jj^(t j —1j9j — Cj9j) (10.19) 

J=I 

Still within the framework of the natural orthogonal complement, we set 
up the relation between the twist ty^ and the vector of actuated joint rates 
q as 

tM = Tq (10.20) 

which upon differentiation with respect to time, yields 

ty4 = Tq+Tq (10.21) 

In the next step, we substitute ty^ and its time-derivative as given by 
eqs. (10.20 & 10.21) into eq.(10.19), thereby obtaining 

My4(Tq + Tq) + Wy^My^Tq 

VI VI 

+ Y, + Cj9j) = + Y (10-22) 

j=i j=i 

Further, we recall relation (10.17), which upon differentiation with re- 
spect to time, yields 

0j = Ljq+Ljq (10.23) 

Next, relations (10.17 & 10.23) are substituted into eq.(10.22), thereby 
obtaining the model sought in terms only of actuated joint variables. After 
simplification, this model takes the form 

My^Tq + My^Tq + Wy^My^Tq 

J=VI VI 

+ Y J7^(ijLjq + IjLjq + CjLjq) = ^ Jy^Tj (10.24) 

J=I J=I 
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where now Ij = Ij(q) and Cj = Cj(q, q). 

Our final step in this formulation consists in deriving a reduced 6x6 
model in terms only of actuated joint variables. Prior to this step, we note 
that from eqs.(10.13), (10.17), and (10.20), 

Lj = J7^T (10.25) 

Upon substitution of the above relation into eq. (10.24) and multiplication 
of both sides of eq. (10.24) by from the left, we obtain the desired model 

in the form of eqs. (10.8a), namely. 



VI 



M(q)q + N(q, q)q = 



(10.26) 



J=I 



with the 6x6 matrices M(q), N(q, q), and vector defined as 



VI 



M(q) = T^MmT + ^ LjljLj 



(10.27a) 



J +/ 



VI 



N(q, q) = T^(MmT + WmMmT) + ^ Viljtj + CjLj) (10.27b) 

. 1=1 

(10.27c) 



Alternatively, the foregoing variables can be expressed in a more compact 
form that will shed more light on the above model. To do this, we define 
the 36 X 36 matrices I and C as well as the 6 x 36 matrix L, the 6x6 
matrix A, and the 6-dimensional vector 4> as 



and hence, 

M(q) 
N(q, q) 

VI 

J=I 



I = diag(I/, I//, . . . , Ivi) 

C = diag(C/, C//, . . . , Cy/ 

L = [ L/ L// . . . Ly/ ] 

A=[L/e/fc L//6//fc ... 

4>=[fi fn ... fvif 

T^M^T + L^IL 

T^(MmT + WmMmT) + L 

A0 



(10.28a) 

(10.28b) 

(10.28c) 

Ly/ey/fc ] (10.28d) 

(10.28e) 

(10.29a) 

aL + L^C(q,q)L (10.29b) 
(10.29c) 



whence the mathematical model of eq. (10.26) takes on a more familiar 
form, namely. 



M(q)q + N(q, q)q = + A0 



(10.30) 
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Thus, for inverse dynamics, we want to determine 0 for a motion given 
by q and q, which can be done from the above equation, namely, 

0 = A-l[M(q)q+N(q,q)q-T^] (10.31) 

Notice, however, that the foregoing solution is not recursive, and since it 
requires linear-equation solving, it is of order n®, which thus yields a rather 
high numerical complexity. It should be possible to produce a recursive 
algorithm for the computation of <p, but this issue will not be pursued here. 
Moreover, given the parallel structure of the manipulator, the associated 
recursive algorithm should be parallelizable with multiple processors. 

For purposes of direct dynamics, on the other hand, we want to solve for 
q from eq.(10.30). Moreover, for simulation purposes, we need to derive the 
state- variable equations of the system at hand. This can be readily done if 
we define r = q, the state- variable model thus taking on the form 

q = r (10.32a) 

r = M^^[— N(q, r)r + + A0] (10.32b) 

In light of the matrix inversion of the foregoing model, then, the complexity 
of the forward dynamics computations is also of order n®. 

Example 10.4.1 Derive matrix hj of eq. (10. 17) for a manipulator having 
six identical legs like that of Fig. 10.2, the actuators being placed at the 
fourth joint. 

Solution: We attach coordinate frames to the links of the serial chain of 
the Jth leg following the Denavit-Hartenberg notation, while noting that 
the first three joints intersect at a common point, and hence, ri = r 2 = ra. 
According to this notation, we recall, vector r* is directed from the origin 
Oi of the *th frame to the operation point of the manipulator, which in this 
case, is The Jacobian matrix of the Jth leg then takes the form 



Jj 



ei 62 63 64 65 

6i X ri 62 X ri 63 X ri 64 x r4 65 x rs 



66 

66 X rs 



J J 



the subscript J of the array in the right-hand side reminding us that the 
vectors inside it pertain to the Jth leg. Thus, matrix Jj maps the joint-rate 
vector of the Jth leg, 6j, into the twist ty 4 of the platform, i.e.. 



Jj^j = ty4 



Clearly, the joint-rate vector of the Jth leg is defined as 

6j = [djl 9 j2 0j3 9j4 6j3 0J6]^ 

Now, note that except for Oj^, all joint-rates of this leg are passive and 
thus need not appear in the mathematical model of the whole manipulator. 
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Hence, we should aim at eliminating all joint-rates from the above twist- 
rate relation, except for the one associated with the active joint. We can 
achieve this if we realize that 



r ji X ejj + ejj x rji = 0 , i = 1 , 2 , 3 
Further, we define a 3 x 6 matrix Aj as 

Aj = [Rji 1 ] 

with Rji defined, in turn, as the cross-product matrix of rji. Now, upon 
multiplication of Jj by Aj from the right, we obtain a 3 x 6 matrix whose 
first three columns vanish, namely, 

AjJj = [0 0 0 e4x(r4-ri) e5x(r5-ri) e6x(r5-ri)]j 

and hence, if we multiply both sides of the above twist-shape equation by 
Aj from the right, we will obtain a new twist-shape equation that is free 
of the first three joint rates. Moreover, this equation is 3 -dimensional, i.e., 

[64 X (r 4 - ri)04 + 65 X (r5 - ri)05 + 65 x {r^ - rij^e] j = i^M x rji + cm 

where the subscript J attached to the brackets enclosing the whole left- 
hand side again reminds us that all quantities therein are to be understood 
as pertaining to the Jth leg. For example, 64 is to be read 6/4. Furthermore, 
only Oji is associated with an active joint and denoted, henceforth, by qj, 
i.e., 

qj = e.M ( 10 . 33 ) 

It is noteworthy that the foregoing method of elimination of passive joint 
rates is not ad hoc at all. While we applied it here to the elimination of the 
three joint rates of a spherical joint, it has been formalized and generalized 
to all six lower kinematic pairs (Angeles, 1994 ). 

We have now to eliminate both Oj^ and Ojq from the foregoing equation. 
This can be readily accomplished if we dot-multiply both sides of the same 
equation by vector uj defined as the cross product of the vector coefficients 
of the two passive joint rates, i.e., 

uj = [65 X (r5 - ri)]j X [66 X (rs - ri)]j 

We thus obtain a third twist-shape relation that is scalar and free of passive 
joint rates, namely. 



uj • [64 X (r4 - ri)6»4]j = uj • {um x rji + cm) 



The above equation is clearly of the form 

Cjqj = y^^M, = (^ 4 )j, J = F/ 
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with Cj and yj defined, in turn, as 



Cj = uj • ej4 X (rj4 - rji) (10.34a) 



yj = 



rji X uj 

uj 



(10.34b) 



Upon assembling the foregoing six scalar twist-shape relations, we obtain 
a 6-dimensional twist-shape relation between the active joint rates of the 
manipulator and the twist of the moving platform, namely. 



Zq = Yty 4 



with the obvious definitions for the two 6x6 matrices Y and Z given below: 



Y = 



yJ- 

yJi 



Z = diag(C/, Cn, • • • , Cvi) 



T 

Yvi 



We now can determine matrix T of the procedure described above, as long 
as Y is invertible, in the form 



T = Y ^Z 



whence the leg-matrix Lj of the same procedure is readily determined, 
namely, 

hj = Jj^T 

Therefore, all we need now is an expression for the inverse of the leg Ja- 
cobian Jj. This Jacobian is clearly full, which might discourage the reader 
from attempting its closed-form inversion. However, a closer look reveals 
that this Jacobian is similar to that of decoupled manipulators, studied 
in Section 4.5, and hence, its closed-form inversion should be reducible to 
that of a 3 X 3 matrix. Indeed, if we recall the twist-transfer formula of 
eqs. (4.62a & b), we can then write Jj as 



Jj = UjKj 



where Uj is a unimodular 6x6 matrix and Kj is the Jacobian of the same 
Jth leg, but now defined with its operation point located at the center of 
the spherical joint. Thus, 



Uj = 



1 o 

Oji - Cm 1 



Kj = 



Kii Ki2 

O K 22 



the superscript J indicating the Jth leg and with the definitions below: 



O: the 3 X 3 zero matrix; 
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1 : the 3x3 identity matrix; 

Oji: the cross-product matrix of oji, the position vector of the center of 
the spherical joint; 

Cy^: the cross product matrix of cy^, the position vector of Cy^. 
Furthermore, the 3x3 blocks of Kj are defined, in turn, as 



(Kii) 


)j = 


[ei 


62 


63 ]j 


(K12: 


)j = 


[64 


65 


66 ]j 


(K22: 


)j = 


[64 


X (r4 


-ri) e 5 x(r 5 -ri) e 6 x(r 5 -ri)]j 



Now, if the inverse of a block matrix is recalled, we have 



K 



-1 

j 



K 



-1 

11 



O 



-K 



uKi2K 

K 22 ' 



-1 1 
22 



J J 



where the superscript of the blocks has been transferred to the whole ma- 
trix, in order to ease the notation. The problem of inverting Kj has now 
been reduced to that of inverting two of its 3 x 3 blocks. These can be in- 
verted explicitly if we recall the concept of reciprocal bases (Brand, 1965). 
Thus, 



(Kn')j 



1 



(62 X 63) 
(63 X ei) 
(ei X 62) 



T' 

T 

T 



J J 



(K2-2')j 



1 

^22 



[(65 X S5) X (ee 
[(ee X S5) X (ey 
[(64 X S4) X (65 



xss)F' 

X S4)]^ 
X ss)]^ 



with Sj 4 , Sj 5 , and A 22 defined as 



SJ4 = r J4 - r J1 
sj5 = r J5 - r Ji 

A44 = det(Ki4) = (ei x 62 • 63) j 

A22 = det(K22) = [(64 X S4) X (6x85) • (eg x 85)]/ 



the subscripted brackets and parentheses still reminding us that all vectors 
involved pertain to the Jth leg. Moreover, since Uj is unimodular, its 
inverse is simply 

u 1 ^10 

CM-0.n 1 



and hence. 



J 



-1 

J 



Kri^ - Kri^Ki2K22HCM - Oji 

K22Cy4 



-K 



uKi2K 

^22 



-1 1 
22 



J J 
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the matrix sought, Lj, then being calculated as 

Lj = jy^Y-^z 

While we have a closed- form inverse of Jj, we do not have one for Y, which 
is full and does not bear any particular structure that would allow us its 
inversion explicitly. Therefore, matrix Lj should be calculated numerically. 



10.5 Dynamics of Rolling Robots 

The dynamics of rolling robots, similar to that of other robotic mechanical 
systems, comprises two main problems, inverse and direct dynamics. We 
will study both using the same mathematical model. Hence, the main task 
here is to derive this model. It turns out that while rolling robots usu- 
ally are nonholonomic mechanical systems, their mathematical models are 
formally identical to those of holonomic systems. The difference between 
holonomic and nonholonomic systems lies in that, in the former, the num- 
ber of independent actuators equals the necessary and sufficient number of 
variables — independent generalized coordinates in Lagrangian mechanics — 
defining a posture (configuration) of the system. In nonholonomic systems, 
however, the necessary and sufficient number of variables defining a posture 
of the system exceeds the number of independent actuators. As a conse- 
quence, in holonomic systems the dof equals the number of independent 
actuators. In nonholonomic systems, the dof is usually defined as the nec- 
essary and sufficient number of variables defining the system posture, while 
the number of independent actuators is termed the system mobility, which 
thus turns out to be smaller than the system dof. Therefore, relations be- 
tween these dependent and independent variables will be needed and will 
be derived in the course of our discussion. Moreover, we will study robots 
with both conventional and omnidirectional wheels. Of the latter, we will 
focus on robots with Mekanum wheels. 

10.5.1 Robots vMh Conventional Wheels 

We study here the robot of Fig. 8.22, under the assumption that it is driven 
by motors collocated at the axes of its two coaxial wheels, indicated as Mi 
and M2 in Fig. 8.22b. For quick reference, we repeat this figure here as 
Fig. 10.6. 

Our approach will be one of multibody dynamics; for this reason, we 
distinguish five rigid bodies composing the robotic mechanical system at 
hand. These are the three wheels (two actuated and one caster wheels), the 
bracket carrying the caster wheel, and the platform. We label these bodies 
with numbers from 1 to 5, in the foregoing order, while noticing that bodies 
4 and 5, the bracket and the platform, undergo planar motion, and hence, 
deserve special treatment. The 6x6 mass matrices of the first three bodies 
are labeled Mi to M3, with a similar labeling for their corresponding 6- 
dimensional twists, the counterpart items for bodies 4 and 5 being denoted 
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FIGURE 10.6. A 2-dof rolling robot: (a) its general layout; and (b) a detail of its 
actuated wheels. 



by M 4 , M 5 , t 4 , and t^, the primes indicating 3 x 3 — as opposed to 6 x 6 
in the general case — mass matrices and 3-dimensional — as opposed to 6 - 
dimensional in the general case — twist arrays. 

We undertake the formulation of the mathematical model of the mechan- 
ical system under study, which is of the general form of eq.( 10 . 8 a) derived 
for holonomic systems. The nonholonomy of the system brings about spe- 
cial features that will be highlighted in the derivations below. 

As a first step in our formulation, we distinguish between actuated and 
unactuated joint variables, grouped into vectors 9a and 0 „, respectively, 
their time-derivatives being the actuated and unactuated joint rates, 9a 
and 9u, respectively. Prom the kinematic analysis of this system in Subsec- 
tion 8 . 6 . 1 , it is apparent that the foregoing vectors are all 2 -dimensional, 
namely. 



9a = 



9,, = 






(10.35) 



Further, we set to deriving expressions for the twists of the five moving 
bodies in terms of the actuated joint rates, i.e., we write those twists as 
linear transformations of 0 „, i.e.. 



ti = Ti9a, i= 1,2,3 (10.36a) 

and 

t'=T'0„, i = 4,5 (10.36b) 

where, from eqs.(8.115a &b), (8.118a &b), and (8.122), 



Ti 



i + yO(5k — yO(5k 

rj 0 



(10.37) 
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T2 

T3 



T' 

T' 

-*-5 



y0(5k — (i + y0(5k) 

0 rj 

©3 

C 3 

C 4 



p 5 

r(Ai+ (l/2)j) 



-p 5 

r(-Ai+ (l/2)j) 





■ 0 r 




.C 5 . 



(10.38) 

(10.39) 

(10.40) 

(10.41) 



with © 3 , C 3 , 04 and C 4 yet to be derived. In the sequel, we will find 
convenient to work with a few nondimensional parameters, a, (5, p — already 
defined in eq.(8.127) — and A, which is introduced now, and displayed below 
with the first three parameters for quick referrence: 



a = 



a + 6 
I 






(10.42) 



In the derivations below, we resort to the notation introduced in Subsec- 
tion 8.6.1. First, we note that, from eqs.(8.119) and (8.126a & b), we can 
write, with 0ij denoting the (*, j) entry of 0, as derived in Subsection 8.6.1, 



<^3 — + ^^ 12 ^^ 2)63 + [pS{ 9 i — 62) + d 2 i 0 i + 022f^2]k (10.43) 



or 

CU 3 = ©30a (10.44) 

with ©3 defined as 



©3 = [6'lie3 + (021 + M)k 01263 + (022 ~ pd)k] 
In more compact form. 





©3 — [01163 + 02 lk 01263 + 022 k] 


(10.45a) 


with 021 and O 22 defined, in turn, as 






021 = 021 + pS, 022 = 022 ~ pS 


(10.45b) 


Moreover, 


C 3 = -r 03 f 3 = -r(0ii0i + 01202)f3 




and hence. 


C 3 = r[— 0 iif 3 — 012 f 3 ] 


(10.46) 


Further, it 


is apparent from Fig. 10.6 that the scalar angular velocity of 



the bracket, is given by 



(414 = Ctl + l/j 
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and hence, 

LV4 = p 5 {i)i — 62) + O21O1 + 922^2 = 92 \ 9 \ + 822^2 



Therefore, we can write 

uJA = el 6 a (10.47a) 

where 64 is defined as 

6>4 = [021 ^22]^ (10.47b) 

Now, since we are given the inertial properties of the bracket in bracket 
coordinates, it makes sense to express 64 in those coordinates. Such an 
expression can be obtained below: 

C4 = 63 + W4 X i[-df 3 + {h- r)k] = -r^3f3 + + ip)e3 

Upon expressing 9^ and tp in terms of 9i and 92 , we obtain 

C4 = d ^^2163 - p6'iif3^ 9i+d ^^2263 - p6'i2f3^ 92 (10.48) 



whence it is apparent that 

C4 = d [(1/2)02163 -P<?llf3 (1/2)02263 -P012f3] (10.49) 

Therefore, 



T' 



021 922 

d[(l/2)02l63 — p01lf3] d[(l/2)02263 — p012f3] 



(10.50) 



thereby completing all needed twist-shaping matrices. 

The 2x2 matrix of generalized inertia, 1 ( 0 ), is now obtained. Here 
we have written this matrix as a function of all variables, independent 
and dependent, arrayed in the 4-dimensional vector 6 , because we cannot 
obtain an expression for 0„ in terms of 9a, given the nonholonomy of the 
system at hand. Therefore, I is, in general, a function of 0i, 02, 03, and 
To be sure, from the above expressions for the twist-shaping matrices Tj 
and T(, it is apparent that the said inertia matrix is an explicit function 
of only, its dependence on 0i and 02 being implicitly given via vectors 
63 and f3. We derive the expression sought for I starting from the kinetic 
energy, namely. 






+ fe(T')^M(TA0„ (10.51) 



or 
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and hence, 



1 4 



(10.52) 



In order to expand the foregoing expression, we let 3^ and Jc be the 
3x3 inertia matrices of the two actuated wheels and the caster wheel, 
respectively, the scalar moments of inertia of the bracket and the platform, 
which undergo planar motion, being denoted by If, and Ip. Likewise, we 
let niiu, rrib, nic, and nip denote the masses of the corresponding bodies. 
Therefore, 



Ml 

Mg 



m; 

M' 





0 


0 


myjl: 


Jc 


0 


0 


'm-cls 


Ib 


0^ ■ 


0 


mbl2_ 


Ip 


0^ 


0 


mplg 



with O and I 3 denoting the 3x3 zero and identity matrices, while 0 and I 2 
the 2-dimensional zero vector and the 2x2 identity matrix. Furthermore, 
under the assumption that the actuated wheels are dynamically balanced, 
we have 






10 0 
OHO 
0 0 H 



Moreover, we assume that the caster wheel can be modeled as a rigid disk of 
uniform material of the given mass nic and radius r, and hence, in bracket- 
fixed coordinates {eg, fg, k}. 



Jc 



2 0 0 
0 1 0 
0 0 1 



It is now a simple matter to calculate 



TfMiTi 



T^MgTg 



I + {pSY H + niyjr'^ 

-{pSfH {p5?H 

{p5fH ~{p5fH 

— {pS)‘^H I + (pS)'^ H + niwr'^ 



where the symmetry between the two foregoing expressions is to be high- 
lighted: that is, the second expression is derived if the diagonal entries of 
the first expression are exchanged, which is physically plausible, because 
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such an exchange is equivalent to a relabeling of the two wheels. The calcu- 
lation of the remaining products is less straightforward but can be readily 
obtained. From the expressions for T 3 and M 3 , we have 



T^M3T3 = [0i^ Cn 



Jc 0 




©3 


_0 mclg_ 




.C 3 . 



0^Jc©3 



mcCg C 3 



In order to calculate the foregoing products, we write Jc and ©3 in com- 
ponent form, i.e.. 



Jc©3 = 



0 

0 

CM 

1 






to 


1 2 

= — mcr 


0 1 0 




0 


0 


0 0 1 




to 


to 

to 





and hence, 



©3 Jc©3 = 



20?i + 021_ 



Likewise, 



"TgCg C 3 = 

Further, 

(T;fM;x; = [04 Cj] 

Upon expansion, we have 






R 1 P 12 



'h 0^ ■ 




'el 


0 m6l2 




_C4_ 



2011 2012 
_0 _0 

021 022 



2011012 + 021022 
2012 T ^22 



RIPI 2 

OI2 



— C 4 



(t;)^ m;t; = /, 



021 021022 
^21022 022 



1 






-2 



'^21 w 4p^0ii 

021022 +4p^011012 



[05 Cl] 



Finally, 

(T')^M'T' 

which can be readily expanded as 



'Ip 0^ ■ 




'of 


0 mpl 2 




.C5. 



021022 + 4p^011012 
022 + 4p^0i2 



+ 'm-pCl C 5 



■ 1 


- 1 " 


+ 


■(l/4) + A 2 


(1/4) -W1 


-1 


1 


_(l/4)-W 


(l/4) + A2_ 



We can thus express the generalized inertia matrix as 
I = I-UI + Ic + I 5 + Ip 
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where 1^^,, Ic, lb, and Ip denote the contributions of the actuated wheels, 
the caster wheel, the bracket, and the platform, respectively, i.e.. 



Ic 

I 5 

Ip 



6 ( 

+ S 21 S 22 



I + 2{pSfH + m^r^ -2{p6fH 

—2(pS)‘^ H I -\- 2{p6Y H -\- niyjr^ 



nicT 



ml. 






(idii6i2 + O 21 O 22 

0 I 2 



m\2 



jli 

(^ 21(^22 



- —nib(f 



ipipsf 



“ 21^22 

^2 

4d 



4p‘^el 



11 



^2X^22 + 4p^011012 

021^22+ V011012 



^22 



12 



■ 1 


- 1 " 


+ rripr'^ 


■(1/4) + A2 


(1/4) -A^l 


-1 


1 


(1/4) -W 


(l/4) + A2_ 



It is now apparent that the contributions of the actuated wheels and the 
platform are constant, while those of the caster wheel and the bracket 
are configuration-dependent. Therefore, only the latter contribute to the 
Coriolis and centrifugal generalized forces. We thus have 



T^MT = + (T;)^M;t; 

From the expression for T^^MaTa, we obtain 

Mata = 0^ Jc©a + maC^ Ca 



the time-derivatives being displayed below: 



®3 — [^^iiea + + ^^2ik <?i2ea + <?i2t^4f3 + ^^22k] 

C3 = r [ — ^iifa + 0iiu;4ea — ^i2f3 + ^^i2‘^46a ] 



with the time-derivatives of the entries of 0 given as 



0 



■ 4 , 



— asini/j + (cos-i/>)/2 asini/j + (cos-i/>)/2 

p\—a cos i/j — (sin i/j) /2] p\a cos i/j — (sin i/j) /2] 



(10.53) 



its parameters being defined in eq.(10.42). Upon expansion, the products 
appearing in the expression for T^^Mata become 



0a^Jc03 



r 26»ii0ii + 021^21 

4 2012011 + 022^21 



"2P\\P\2 + (h\^22 

2 O 12 O 12 + O 22 O 22 



«iaCg Ca = mcr^ 



\lSll 011012 
12^^11 (^ 12(^12 



Therefore, 



T^Mgtg 



6011011 + ^21021 
6012^^11 + (^22(^21 



6011012 + S 21 S 22 
6012^^12 + 622622 



4 
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Likewise, 

+ nibCjCi 

the above time-derivatives being 
^4 = [^21 ^22 ] 

C4 = d[ciie3 + Ci2f3 C2ie3+C22f3] 
with coefficients Cj ^ given below: 

Cll = -<^21 + d^^ll‘^4 , C12 = -<?21‘^4 — 

C 2 I = 2^22 + p(^l2^A ^ C 22 = —(^22^A ~ P^\2 

Hence, 



^4 



^ 21^^21 

^ 22(^21 



(^ 21(^22 

S 22 S 22 



mbC\ C4 = -mbd 



^2lCll — 2p9iiCi2 
S 22 CII — 2p9i2Ci2 



' 21 C 2 I — 2p9\\C22 
'22C2I — 2p9\2C22 



Therefore, 






h 



821^21 

(^ 22(^21 



-nibdr 



(^ 21(^22 
S 22 S 22 _ 

' 21 C 11 — 2p9nci2 
' 22 C 11 — 2p9i2C\2 



'21C2I — 2p9\\C22 
' 22 C 2 I — 2p9\2C22 



In the final steps, we calculate T^WMT. As we saw earlier, only the 
caster wheel and the bracket can contribute to this term, for the contribu- 
tions of the other bodies to the matrix of generalized inertia are constant. 
However, the bracket undergoes planar motion, and according to Exer- 
cise 6 . 8 , its contribution to this term vanishes. Therefore, 



T^WMT = TTW3M3T3 



Upon expansion of the foregoing product, we have 



T^W3M3T3 






Gl\ 



^3 O' 




Ic 0 




©3 


0 0 




_0 mcl3_ 




.C 3 . 



©3 ri3lc© 



First, we obtain ^3 in bracket coordinates, by recalling eq.(10.43), i.e., 

[<^3 ]a = ‘^063 + u;fck 
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with LOe and lOk denoting the nonzero components of in bracket coordi- 
nates, i.e., 



= pS{ 0 \ — O 2 ) + + O 22 O 2 



and hence, 



^^3 — + ^^12^^2)E3 + [pS{9i — 62 ) + d2l0l + <?22^^2]K 

with E3 and K defined as the cross-product matrices of 63 and k, respec- 
tively, that is. 



'0 


0 


0 ■ 




'0 


-1 


o' 


0 


0 


-1 


, K = 


1 


0 


0 


1 

0 


1 


0 




1 

0 


0 


1 

0 



Therefore, 






After simplification. 



0 —kJk 0 

LUk 0 (jJq 

Q ujp Q 



^^ 3 lc ®3 



nicr^ 

4 



0 _ 0 _ 

29iiU)k — 92\LOp 29i2LOk — (^22^e 

0 0 



Now it is a simple matter to verify that 

0 ^r 23 lc ©3 = O 2 

with O2 denoting the 2x2 zero matrix, and hence, 

T^WT = O2 



In summary, the Coriolis and centrifugal force terms of the system at hand 
take the form 






Trier‘S Q9ii{9i\9i + 9\2^2) + + ^ 22 ^ 2 ) 

4 6012(01101 + ^12^2') + d22{9\29i + 92262) 



+ h{ 92 idi + 92262) 



'21 

*22 



+ -m6d^(cii0i + C2102) 



21 

'22 



-mk(f p{ci 29 i + 02262) 



11 

'12 



If we recall that the Cij coefficients are linear in the joint rates, then the 
foregoing expression clearly shows the quadratic nature of the Coriolis and 
centrifugal terms with respect to the joint rates. 
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The derivation of the forces supplied by the actuators is straightforward: 



T 



Tl 

T2 



The dissipative generalized force is less straightforward, but its calculation 
is not too lengthy. In fact, if we assume linear dashpots at all joints, then 
the dissipation function is 






1 -T 

2^aCuea 



-On C340„ 



with C 12 and C 34 defined as 



C12 



Cl 0 

0 C2 



C34 



C 3 0 
0 C4 



Now, if we recall the expression for in terms of 6a, we end up with 

A=^0rD0„ 

D being defined, in turn, as the equivalent damping matrix, given by 

D = C 12 + 0 ^C 34 © 

the d}mamics model under study thus taking the form 

l{0)'0a + C{0,0a)ea=r-T>0a 

with I and C(0, 0a) given, such as in the case of holonomic systems, as 

I(6>) = T^MT 

C( 6 >, 0 a) = T^MT + T^WMT 

thereby completing the mathematical model governing the motion of the 
system at hand. Note here that 0 denotes the 4-dimensional vector of joint 
variables containing all four angles appearing as components of 0a and . 
Because of the nonholonomy of the system, an expression for the latter in 
terms of the former cannot be derived, and thus the whole 4-dimensional 
vector 0 is left as an argument of both I and C. 

Note that calculating the torque r required for a given motion — inverse 
dynamics — of the rolling robot under study is straightforward from the 
above model. However, given the strong coupling among all variables in- 
volved, a recursive algorithm in this case is not apparent. On the other 
hand, the determination of the motion produced by a given history of joint 
torques requires (*) the calculation of I, which can be achieved s}miboli- 
cally; (m) the inversion of I, which can be done symbolically because this 
is a 2 X 2 matrix; (ni) the calculation of the Coriolis and centrifugal terms, 
as well as the dissipative forces; and )iv) the integration of the initial- value 
problem resulting once initial values to 0 and 0a have been assigned. 
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10.5.2 Robots with Omnidirectional Wheels 



We now consider a 3-dof robot with three actuated wheels of the Mekanum 
type, as shown in Figs. 1.11a and 8.24, with the configuration of Fig. 10.7, 
which will be termed, henceforth, the A-array. This system is illustrated 
in Fig. 10.8. 

Below we will adopt the notation of Subsection 8.6.2, with a = tt/ 2 and 
n = 3. We now recall that the twist of the platform was represented in 
planar form as 



t' 



LU 

c 



(10.54) 



where lu is the scalar angular velocity of the platform and c is the 2- 
dimensional position vector of its mass center, which will be assumed to 
coincide with the centroid of the set of points {Ci}^. Moreover, the three 
wheels are actuated, and hence, the 3-dimensional vector of actuated joint 
rates is defined as 



0 



a 



'1 

h 

*3 



(10.55) 



The relation between 9a and t' was derived in general in Subsection 8.6.2. 
As pertaining to the robot of Fig. 10.7, we have 



30a = Kt' 



(10.56a) 



with the two 3x3 Jacobi ans J and K defined as 



J = — al, K = 



ff 



(10.56b) 



where, it is recalled, a is the height of the axis of the wheel hub and r 
is the horizontal distance of the points of contact with the ground to the 
mass center C of the platform, as indicated in Fig. 10.7a. Moreover, vectors 
{ej}® and {fj}i, defined in Subsection 8.6.2, are displayed in Fig. 10.7. 
Below we derive expressions for uj and c, from eq.(10.56a), in terms of the 
joint rates. To this end, we expand these three equations, thus obtaining 

ruj + flf c = —a9i (10.57a) 

rct; + f Jc = —002 (10.57b) 

ruj + c = —abs (10.57c) 



Upon adding corresponding sides of the three foregoing equations, we 
obtain 

3 3 

3rco + fj = — o 0j 

1 1 
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FIGURE 10.7. Rolling robot with ODWs in a A-array. 



But from Fig. 10.7b, it is apparent that 







ei + 62 + eg = 0 

fl + fg + fg = 0 




(10.58a) 

(10.58b) 


Likewise, 










ei = 


^(f 3 - h), 


62 = ^(fl - fs), 


63 = ^(f2 - fl) 


(10.58c) 


fi = 


^(62 - eg) 


, f2 = ^(63 -6l) 


, f 3 = ^(61 -62) 


(10.58d) 



and hence, the above equation for to and c leads to 

3 

= (10-59) 



Now we derive an expression for c in terms of the actuated joint rates. We do 
this by subtracting, sidewise, eq. (10.57b) from eq. (10.57a) and eq.(10.57c) 
from eq.(10.57b), thus obtaining a system of two linear equations in two 
unknowns, the two components of the 2-dimensional vector c, namely, 

Ac = b 



with matrix A and vector b defined as 



A = 



(fl-f2)^ 

(f2 - fsV 






b = —a 



1 — 
'2 - 



2 

*3 
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FIGURE 10.8. A view of the three-wheeled robot with Mekanum wheels in a 
A-array. 

where we have used relations (10.58c). Since A is a 2 x 2 matrix, its inverse 
can be readily found with the aid of Facts 4.8.3 and 4.8.4, which yield 

2 

c = -a[-Eei Eea] 

Now, from Fig. 10.7b, 

Eei = fi, Eea = fa 

and hence, c reduces to 

C = ^a[(<i2 - ei)h + (02 - 03)f3] = ^a[02(fl + f3) - 0lfl - 03f3] 

But by virtue of eq. (10.58b), 

fi + f3 = —h 

the above expression for c thus becoming 

r, 3 

C= (10.60) 

1 

Thus, uj is proportional to the mean value of { Oi }®, while c is proportional 
to the mean value of { 0jfi }®. In deriving the mathematical model of the 
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robot at hand, we will resort to the natural orthogonal complement, and 
therefore, we will require expressions for the twists of all bodies involved in 
terms of the actuated wheel rates. We start by labeling the wheels as bod- 
ies 1, 2, and 3, with the platform being body 4. Moreover, we will neglect 
the inertia of the rollers, and so no labels need be attached to these. Fur- 
thermore, the wheel hubs undergo rotations with angular velocities in two 
orthogonal directions, and hence, a full 6 -dimensional twist representation 
of these will be required. Henceforth, we will regard the angular velocity of 
the platform and the velocity of its mass center as 3-dimensional vectors. 
Therefore, 



t 4 = T40 



a: 



T4 = -A 



k k k 

2 rfi 2rf2 2 rfg 



(10.61) 



with A defined, in turn, as the ratio 



a 






2 >r 



(10.62) 



Now, the wheel angular velocities are given simply as 






4k = OiBi — A 




(10.63) 



or 



u)\ = (ei — Ak)<ii — A<i 2 k — Allak (10.64a) 

u>2 = — Aliik + (b 2 — Ak)<i 2 — Aliak (10.64b) 

W 3 = — Aliik — A<i 2 k + (63 — Ak)<i 3 (10.64c) 



Similar expressions are derived for vectors Cj. To this end, we resort to the 
geometry of Fig. 10.7, from which we derive the relations 

/ 3 \ / 3 \ 



Cj = c + ijjrfi 



2 Ar 



Ar 






and hence. 



Cl = -\r[{Wi + 02+ <?3)fi + 2(02f2 + Oak)] (10.65a) 

C 2 = -Ar[2<iifi + (<ii + 302 + 0a)k + 203 ^] (10.65b) 

C 3 = -Ar[2(<iifi + <i 2 f 2 ) + {0i + 02 + 3<?3)f3] (10.65c) 



From the foregoing relations, and those for the angular velocities of the 
wheels, eqs.(10.64a-c), we can now write the twists of the wheels in the 
form 



tj = Tj6>„, * = 1,2,3 



( 10 . 66 ) 
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where 



Ti 

T 2 

T 3 



ei — Ak — Ak 

-3Arfi -Ar(fi+ 2 f 2 ) 

— Ak 62 — Ak 

-Ar(f 2 + 2fi) - 3 Arf 2 



-Ak 

Ar(fi + 2fa) 
-Ak 

Ar(f2 + 2fa) 



-Ak 

-Ar(f 3 + 2fi) 



— Ak 63 — Ak 

Ar(f3+2f2) -3Arf3 



On the other hand, similar to what we have in eq. (10.60), an interesting 
relationship among angular velocities of the the wheels arises here. Indeed, 
upon adding the corresponding sides of the three equations (10.64a-c), we 
obtain 

33 3 

1 1 1 

Further, we dot-multiply the two sides of the foregoing equation by k, which 
yields, upon rearrangement of terms. 



3 3 

1 1 



and by virtue of eq.(10.59). 



UJ 



k • (jj, 





(10.67) 



that is, the vertical component of the mean wheel angular velocity equals 
the scalar angular velocity of the platform. 

Now we proceed to establish the mathematical model governing the dy- 
namics of the system under study. The generalized inertia matrix is then 
calculated as 

4 

I = ^TfMiTi (10.68) 

1 

where, if 1 ^^, and denote the moment-of-inertia matrix — in body-fixed 
coordinates — and the mass of each of the three wheels, with similar defini- 
tions for Ip and as pertaining to the platform. 



Mi 



Iw o 

O niyjl ’ 



i = l,2,3, M 4 



Ip o 

O mpl 



(10.69) 



We will also need the angular- velocity dyads, Wj, which are calculated as 



Wi 



f2i O 
O O 



i = 1,2,3 



(10.70) 
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where W 4 will not be needed, since the platform undergoes planar motion. 
We have 

■j\,r rp Ak) AI.(^k AI.(^k 

^ ^ _ -^niyjXrii -myjXr{ii +2i2) -m^^,Ar(fi + 2fa) _ 

Moreover, we assume that in a local coordinate frame {oj, fj, k}, 

■/ 0 0 
= 0 J 0 

0 0 J 

in which I and J are constants. Hence, 

\I + X'^K AV AV 
TfMiTi= AV X^L X^M 
[ AV A^M X^L 

where 

K = J + 9m^i,r^ 

L = J + 3m^i,r^ 

M = J — Zmyjr-^ 



Likewise, 

r A^L AV A^M' 
T^M2T2= AV I+X^K AV 
A^M AV X^L 

'X^L AV AV 1 

T|’M3T3= AV A^L AV 

[aV AV J + A^iC 

Furthermore, 

Ipk Ipk Ipk 

2 mprfi 2mpri2 2mprf^ 



1VI4X4 — — A 



It is apparent that, by virtue of the planar motion undergone by the plat- 
form, only its moment of inertia H about the vertical passing through its 
mass center is needed. Then, 



H + AnipT 
TJM 4 T 4 = X? H — 2mpr 
H — 2mpr 



H — 2mpr^ H — 2mpr‘^ 
H + AmpT-^ H — 2mpr‘^ 
H — 2mpr^ H + Ampr"^ 



Upon summing all four products computed above, we obtain 



I 



a l3 l3 

fi a. fi 

fi fi a. 
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with the definitions below: 

a = I + }?{H + 3 J + Ibm-ujr^ + 4mj,r^) 

(3 = (iJ + 3 J — 3m^i,r^ — 2mpr^) 

which is a constant matrix. Moreover, note that the geometric and inertial 
symmetry assumed at the outset is apparent in the form of the foregoing 
inertia matrix, its inverse being readily obtained in closed form, namely, 

^ o; R — [3 — [3 

= — —f3 o. + f3 —f3 , A = (a + /3 )q: — 2/3^ 

^ -13 -13 a + l3 



Next, we turn to the calculation of the T^MT term. This is readily found 
to be 

4 

T^MT = 

1 

each of the foregoing products being expanded below. We have, first, 

Lvfl 0 0 

3 Arct;ei —Xruj{es — 62) Xruj{e^ — 62) 

0 Ujf2 0 

Xruj{ei — 63 ) ?>Xruje2 —Xruj{ei — 63 ) 

0 0 Lvfa ' 

—Xruj{e2—ei) Xruj{e2—ei) ^Xnue^ 

■ 0 0 o' 

2nuei 2nue2 2ruje^ 

Hence, for the first wheel. 




Miti 



0 0 

^Xm-u,rujei —Xm-u,ruj{es—e2) Xm-u,ruj — 62) 



Therefore, 



TfMiti 



3%/3A^m^i,r^ct; 



0 

1 

-1 



-1 

0 

0 



1 

0 

0 



where the skew-symmetric matrix is the cross product matrix of vector 
[0, 1, 1]^. By symmetry, the other two products, T^MjTj, for i = 1,2, 
take on similar forms, with the skew-s}mimetric matrix, becoming, corre- 
spondingly, the cross-product matrix of vectors [1, 0, 1]^ and [1, 1, 0]^. 
This means that the first of these three products is affected by the rotation 
of the second and the third wheels, but not by that of the first one; the 
second of those products is affected by the rotation of the first and the 
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third wheels, but not by the second; the third product is affected, in turn, 
by the rotation of the first two wheels, but not by that of the third wheel. 
We thus have 



T^M2T2 = 



Tl'Ma'i'a = lu 



0-10 
1 0 -1 

0 1 0 

0 0 1 

0 0-1 
-1 1 0 



Furthermore, 



1VI4X4 — A 



0 

2mprujei 



0 

2mprujB2 



0 

2mpruje^ 



and hence. 



TJM 4 T 4 = 2%/3A^ mpr^ct; 



0 -1 1 

1 0 -1 

-1 1 0 



(10.72a) 



whose skew-symmetric matrix is readily identified as the cross-product ma- 
trix of vector [1, 1, 1]^, thereby indicating an equal participation of all 
three wheels in this term, a rather plausible result. Upon adding all four 
products calculated above, we obtain 



T^MT = 2%/3A^(3m^i, + mp)r‘^uj 



0 -1 1 

1 0 -1 

-1 1 0 



(10.73) 



The equal participation of all three wheels in the foregoing product is ap- 
parent. Moreover, notice that the term in parentheses can be regarded as 
an equivalent mass, which is merely the sum of all four masses involved, 
the moments of inertia of the wheels playing no role in this term. 

We now turn to the calculation of the T^WMT term, which can be 
expressed as a sum, namely, 

3 

T^WMT = 

1 

where we have not considered the contribution of the platform, because 
this undergoes planar motion. Moreover, matrices Wj, for * = 1,2, and 3, 
take the obvious forms 

Ui O 

o o 



Wi = 
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We then have, for the first wheel, 

WiMiTi=^ 0 0 0 _ 

Now, it does not require too much effort to calculate the complete first 
product, which merely vanishes, i.e.. 



W iMiTi — ©66 

with ©66 defined as the 6x6 zero matrix. By symmetry, the remaining 
two products also vanish, and hence, the sum also does, i.e., 

T^WMT = ©66 (10.74) 

Now, calculating the dissipative and active generalized forces is straight- 
forward. We will neglect here the dissipation of energy occurring at the 
bearings of the rollers, and hence, if we assume that the lubricant of the 
wheel hubs produces linear dissipative torques, then we have 





■0r 




n 


6 = c 


02 


, T = 


T2 




1 

CO 

1 




W3. 



(10.75) 



where c is the common damping coefficient for all three wheel hubs. We now 
have all the elements needed to set up the mathematical model governing 
the dynamics of the robot, namely. 



Wa + C{u;)ea = T -6 (10.76) 

where C(ct;) = T^MT + T^WMT; from eqs.(10.73) and (10.74), this term 
becomes 



C(ct;) = 2%/3A^(3m^i, + mp)r‘^uj 



0 -1 1 

1 0 -1 

-1 1 0 



(10.77) 



Since uj = — a/(3r)(<?i + 02 + 0a), the quadratic nature of the second term 
of eq.(10.76) in the joint rates becomes apparent. It is also apparent that 
the mathematical model derived above does not depend on 6a- What this 
means is that the mathematical model allows the integration of the actu- 
ated joint accelerations to yield joint-rate histories 6a{t), but this model 
cannot provide joint- variable histories 6a{t). To obtain these, for given ini- 
tial conditions, the joint-rate histories have to be integrated, which can be 
done by numerical quadrature. 

Finally, in order to obtain the Cartesian histories of the platform pose, 
given by the angle a that a specific line of the platform makes with a 
line fixed in an inertial frame, and the position vector of the mass center. 
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c, eqs. (10.59) and (10.60) have to be integrated. While the integration of 
the former can be readily done by quadrature, that of the latter requires 
knowledge of vectors fj, for i = 1,2, 3, and these vectors depend on a. Thus, 
the integration of eq.(10.59) can be done once the joint-rate histories are 
known; that of eq.(10.60) requires knowledge of angle a. These features are 
inherent to nonholonomic systems. 




A 

Kinematics of Rotations: A Snmmary 



The purpose of this appendix is to outline proofs of some results in the 
realm of kinematics of rotations that were invoked in the preceding chap- 
ters. Further details are available in the literature (Angeles, 1988). 

We start by noticing two preliminary facts whose proof is straightfor- 
ward, as the reader is invited to verify. 

Lemma A.l The {d/dt){-) and the vect( • ) operators, for 3x3 matrix 
operands, commute. 

and 

Lemma A. 2 The {d/df){-) and the tr( • ) operators, for n x n matrix 
operands, commute. 

Furthermore, we have 

Theorem A.l Let A and S both &e 3 x 3 matrices, the former arbitrary, 
the latter skew- symmetric. Then, 

vect( SA ) = i[tr(A)l — A]s 



where s = vect( S ). 

Proof: An invariant proof of this theorem appears elusive, but a compo- 
nentwise proof is straightforward. Indeed, let Oij denote the (*, j) entry of 
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A, and Sj the *th component of s. Then, 



SA 



— 02153 + 03152 
01153 — aaisi 

— aiiS 2 + 02151 



— 02253 + 03252 
01253 — 03251 

— 01252 + 02251 



— O 2353 + O 33 S 2 

01353 — O33S1 

— 01352 + 02351 



Hence, 

(022 + 033)51 — 01252 — O13S3 
(oil + 033)52 — 02151 — O 2353 
(oil + 022)53 — 03151 — 03252 

On the other hand. 



vect( SA ) 



tr( A )1 — A 



022 + 033 — 012 

— O2I Oil + O33 
— 031 — 032 



— 013 

— 023 

Oil + 022 



and hence, 

i[tr(A)l-A]s = 



Y I (022 + 033)51 — 01252 — 01353 
(oil + 033)52 — 02151 — O2353 
(oil + 022)53 — 03151 — 03252 



thereby completing the proof. Moreover, we have 

Theorem A. 2 Let A, S, and s be defined as in Theorem A.l. Then, 



tr( SA ) = —2s • [vect( A )] 



Proof: Prom the above expression for SA, 



tr(SA) 



— O2153 + 03152 + 01253 — 03251 — 01352 + 02351 



(023 — 032)51 + (031 — 013)52 + (012 — 021)53 



[51 52 53 



O 23 — O 32 
O 3 I — 013 
O 12 — O 21 



2s • [vect( A )] 



(A.l) 



q.e.d. 

Now we turn to the proof of the relations between the time-derivatives 
of the rotation invariants and the angular- velocity vector. Thus, 

Theorem A. 3 Let v denote the f-dimensional array of natural rotation 
invariants, as introduced in Section 2.3.2 and reproduced below for quick 
reference: 

e 
<(> 



u = 
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Then the relationship between z> and the angular velocity u) is given by 

if = Ntu 



with N defined as 



N = 



[sm</)/(2(l — cos(/)))](l — ee^) — (1/2)E 



Proof: Let us obtain first an expression for e. This is readily done by re- 
calling that e is the real eigenvector of Q, i.e., 

Qe = e 

Upon differentiation of both sides of the foregoing equation with respect to 
time, we have 

Qe + Qe = e 



i.e., 

(1 - Q)e = Qe 

An expression for Q can be derived from eq.(3.46), which yields 



Q = f2Q 



(A.2) 



Therefore, 



Qe = Pte = u! X e 

and hence, the above equation for e takes the form 



(1 — Q)e = u) X e 

from which it is not possible to solve for e because matrix (1 — Q) is singular. 
Indeed, since both matrices inside the parentheses have an eigenvalue +1, 
their difference has an eigenvalue 0, which renders this difference singular. 
Thus, one more relation is needed in order to be able to determine e. This 
relation follows from the condition that ||e|p = 1. Upon differentiation of 
both sides of this condition with respect to time, we obtain 

e^e = 0 



the last two equations thus yielding a system of four scalar equations to 
determine e. We now assemble these equations into a single one, namely, 

Ae = b 



where A is a 4 x 3 matrix, while b is a 4-dimensional vector, defined as 
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The foregoing overdetermined system of four equations in three unknowns 
now leads to a system of three equations in three unknowns if we multiply 
its two sides by from the right, thereby producing 

A^Ae = A^b 

We can therefore solve for e from the above equation in the form 

e= (A^A)^^A^b 



where A^A takes the form 

A^A = (2)1 - (Q + Q^) + ee^ 

But the sum inside the parentheses is readily identified as twice the sym- 
metric component of Q, if we recall the Cartesian decomposition of matrices 
introduced in eq.(2.56). Therefore, 



Q + = 2 [(cos <())1 + (1 — cos (j))ee^] 



Hence, 



A^A = 2(1 — cos 4>)\ — {\ — 2 cos 4>)ee^ 

As the reader can readily verify, the inverse of this matrix is given by 



T A 



(A^ A 



1 



1—2 cos d) rp 
-1 4 ^ -ree^ 



2(1— cos())) 2(1— cos())) 



which fails to exist only in the trivial case in which Q becomes the identity 
matrix. Upon expansion of the last expression for e, we have 



1 



-(E- Q^E)w 



2(1 — cos (j)) 

Now Q^E is obtained by recalling eq.(2.54), thereby obtaining 
Q^E = (cos + (sin <)))(1 — ee^) 
the final expression for e thus being 
1 



2(1 — cos 4>) 



[(1 — cos()))E — (sin<)))(l — ee^ )]tu 



Now, an expression for <f> is obtained upon equating the trace of the two 
sides of eq.(A.2), which yields 



From Lemma A. 2, 



tr( Q ) = tr( f2Q [ 



tr(Q) = )^tr(Q[ 



(A.3) 

(A.4) 
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and hence, 



tr( Q ) = — 2<^sin</) 



On the other hand, from Theorem A. 2, 



tr( f2Q ) = —2uj ■ (sin (j))e 



Therefore, 



4> = u; ■ e 



Upon assembling the expressions for e and <f>, we obtain the desired relation, 
with N given as displayed above, thereby proving the theorem. 

Theorem A. 4 Let A denote the 4-dimensional array of linear rotation 
invariants, as introduced in Section 2.3.3 and reproduced below for quick 
reference: 

^ ^ r(sin<())el r vect(Q) 1 



[ cosf \ [[tr(Q) - l]/2j 

Then the relationship between A and the angular velocity is given by 

A = TjCO 



with L defined as 



^ ■(l/2)[tr(Q)l-Q]' 
— (sin (}))e^ 



Proof: Prom Lemma A.l, we have 



— vect( Q ) = vect( Q ) (A. 5) 

On the other hand, equating the vectors of the two sides of eq.(A.2) yields 



and hence. 



vect( Q ) = vect( L2Q ^ 



— vect( Q ) = vect( L2Q 
dt 



But, if we recall Theorem A.l, the foregoing relation leads to 
^vect( Q ) = i[tr(Q)l - Q]w 



Likewise, from Lemma A. 2, we have 



^tr( Q ) = tr( Q ) 
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and hence, 



— tr(Q) =tr(f2Q; 



Now, if we recall Theorem A. 2, the foregoing relation leads to 



— tr( Q ) = — 2tu • [vect( Q )] = — 2(sin</>)e^tu 



Hence, 



— (cos</>) = —{sm(f))e^u! 

Upon assembling the last two expressions for the time-derivatives of the 
vector of Q and cos <f>, we obtain the desired relation. 



Theorem A. 5 Let r) denote the 4-dimensional array of the Euler-Rodri- 
gues parameters of a rotation, as introduced in Section 2.3.6 and reproduced 
below for quick reference: 

r 

ro_ 

Then, the relationship between r) and the angular velocity takes the form 



V = 



[sin((/)/2)]e 

cos(</)/2) 



77 = Hu! 



with H defined as 

rol - R 
— 

where R is the cross-product matrix ofr. 

Proof: If we differentiate r, we obtain 

r = esm 1^1+ \ ^ j 



1 


cos((/)/2)l — sin((/)/2)E 


_ 1 


2 


— sin((/)/2)e^ 


^ 2 



and hence, all we need to derive the desired relations is to find expressions 
for e and f in terms of the Euler-Rodrigues parameters. Note that from 
Theorem A. 3, we already have those expressions in terms of the natural 
invariants. Thus, substitution of the time-derivatives of the natural invari- 
ants, as given in Theorem A. 3, into the above expression for r leads to 



1 1 
— sm — Ecu 4 — sm — 

2 V 2 / 2 V 2 



-(e • tu) 

2^ ’ 



sin f 

(j 

1 — cos 4> 
sin (f> 



^ 4 >\ ■ ( 4 > 

2 ) V2/ 1 - cos(/>J 



(A.6) 
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Now, by recalling the identities giving the trigonometric functions of 4> in 
terms of those of we obtain 



<j) \ sm <j) 

sin I — 

2 y f — cos 4> 



I 

= cos|- 



and hence, the term in brackets of the above expression vanishes, the ex- 
pression for r thus reducing to 



r 



2 



1 



sin 



2 



E 



(jj = 




R)w 



thereby completing the proof. 




B 



The Numerical Solution of Linear 
Algebraic Systems 



In this appendix we consider the solution of the linear algebraic system 

Ax = b (B.l) 

with A defined as a full-rank m x n matrix, while x and b are n- and m- 
dimensional vectors, respectively. The case m = n is the most frequently 
encountered; this case is well documented in the literature (Dahlquist and 
Bjorck, 1974; Golub and Van Loan, 1989) and need not be further discussed. 
We will consider only the following two cases: 



(a) over determined: m > n; and 

(b) underdetermined: m < n. 

The overdetermined case does not admit a solution, unless vector b hap- 
pens to lie in the range of A. Besides this special case, then, we must 
reformulate the problem, and rather than seeking a solution of eq.(B.l), we 
will look for an approximation of that system of equations. Moreover, we 
will seek an approximation that will satisfy an optimality condition. 

The underdetermined case, on the contrary, admits infinitely many so- 
lutions, the objective then being to seek one that satisfies the system of 
equations and satisfies an additional optimality condition as well. 

We study each of these cases in the sections below. 
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B.l The Overdetermined Case 



The error e in the approximation of eq.(B.l) is defined as 

e = b — Ax (B.2) 



An obvious way of imposing an optimality condition on the solution x is 
to specify that this solution minimize a norm of e. All norms of e can be 
expressed as 

/ m \ Cf> 

IN6s(-E4j (B.3) 

with Cfc being understood as the A:th component of the m-dimensional vec- 
tor e. When p = 2, the foregoing norm is known as the Euclidean norm, 
which we have used most frequently in this book. When p ^ oo, the in- 
finity norm, also known as the Chebyshev norm, is obtained. It turns out 
that upon seeking the value of x that minimizes a norm of e, the simplest 
is the Euclidean norm, for the minimization of its square leads to a linear 
system of equations whose solution can be obtained directly, as opposed to 
iteratively. Indeed, let us set up the minimization problem below: 

^(x) = ^||e||^ ^ min (B.4) 

2 X 

The normality condition of the minimization problem at hand is derived 
upon setting the gradient of z with respect to x equal to zero, i.e.. 



dz 

dx 



(B.5) 



Now, the chain rule and the results of Subsection 2.3.1 allow us to write 



dz 

dx 



de \ ^ dz 
dx ) de 



-A^e 



(B.6) 



and hence, we have the first result: 

Theorem B.1.1 The error in the approximation of eq.(B.l), for the full- 
rank m X n matrix A, with m > n, is of minimum Euclidean norm if it 
lies in the nullspace o/A^. 

Furthermore, if eq.(B.2) is substituted into eq.(B.6), and the product 
thus resulting is substituted, in turn, into the normality condition, we ob- 
tain 

A^Ax = A^b (B.7) 

which is known as the normal equation of the minimization problem at 
hand. By virtue of the assumption on the rank of A, the product A^A is 
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positive-definite and hence, invertible. As a consequence, the value Xq of x 
that minimizes the Euclidean norm of the approximation error of the given 
system is 

xo = (A^A)-^A^b (B.8) 

the matrix coefficient of b being known as a generalized inverse of A. The 
error obtained with this value is known as the least-square error of the 
approximation, i.e., 

6o = b — Axo (B.9) 

The reader should be able to prove one more result: 

Theorem B.l. 2 (Projection Theorem) The least-square error is or- 
thogonal to Axq. 

While the formula yielding the foregoing generalized inverse is quite sim- 
ple to implement, the number of floating-point operations (flops) it takes 
to evaluate, along with the ever-present roundoff errors in both the data 
and the results, renders it not only inefficient, but also unreliable if ap- 
plied as such. Indeed, if we recall the concept of condition number, in- 
troduced in Section 4.9 and recalled in Subsection 8.2.4, along with the 
definition adopted in the latter for the condition number, it becomes ap- 
parent that the condition number of A^A is exactly the square of the 
condition number of A. This result can be best understood if we apply the 
Polar-Decomposition Theorem, introduced in Section 4.9, to rectangular 
matrices, but we will not elaborate on this issue here. 

As a consequence, then, even if A is only slightly ill-conditioned, the 
product A^A can be catastrophically ill-conditioned. Below we outline 
two procedures to calculate efficiently the least-square approximation of 
the overdetermined system (B.l) that preserve the condition number of A 
and do this with a low number of flops. 

B.1.1 The Numerical Solution of an Overdetermined 
System of Linear Equations 

In seeking a numerical solution of the system of equations at hand, we would 
like to end up with a triangular system, similar to the LU-decomposition 
applied to solve a system of as many equations as unknowns, and hence, 
we have to perform some transformations either on the rows of A or on 
its columns. A safe numerical procedure should thus preserve (a) the Eu- 
clidean norm of the columns of A and (b) the inner product between any 
two columns of this matrix. Hence, a triangularization procedure like LU- 
decomposition would not work, because this does not preserve inner prod- 
ucts. Obviously, the transformations that do preserve these inner products 
are orthogonal, either rotations or reflections. Of these, the most popular 
methods are (a) the Gram-Schmidt orthogonalization procedure and (b) 
Householder reflections. 
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The Gram-Schmidt procedure consists in regarding the columns of A as 
a set of n m-dimensional vectors {a.k }". From this set, a new set {es, }" 
is obtained that is orthonormal. The procedure is quite simple and works 
as follows: Define ei as 



ei 



ai 

llaill 



(B.IO) 



Further, we define as the normal component of sl2 onto 62 , as introduced 
in eq.( 2 . 6 b), i.e.. 



b 2 = (1 - eief )a 2 (B.lla) 

In the next step, we define 63 as the unit vector normal to the plane defined 
by ei and 62 and in the direction in which the inner product ■ aa is 
positive, which is possible because all vectors of the set { aj, }™ have been 
assumed to be linearly independent — remember that A has been assumed 
to be of full rank. To this end, we subtract from aa its projection onto the 
plane mentioned above, i.e., 

ba = (1 - eief - eae^jaa (B.12a) 

and so on, until we obtain e„_i, the last unit vector of the orthogonal set, 
e„, being obtained as 

b„ = (1 - eief - 620 ^ e„_ie^_i)a„ (B.13a) 



Finally, 




(B.13b) 



In the next stage, we represent all vectors of the set { aj, }" in orthogonal 
coordinates, i.e., in the base O = {ek }", which are then arranged in an 
mxn array Aq. By virtue of the form in which the set { Ok }" was defined, 
the last m — k components of vector a.k vanish. We thus have, in the said 
orthonormal basis, 

aifc 

Oi2k 



[afc ]o 



O-kk 

0 



(B.14a) 



0 
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Further, we represent b in O, thus obtaining 



[b]o 



r/3ii 

/?2 

- /^m - 



Therefore, eq.(B.l), when expressed in O, becomes 



Oil 


ai 2 


^In 








r 1 


0 


0:22 


Oi2n 




■ Xl ■ 




/?2 










X2 




fln 


0 


0 


' ' ' ‘^nn 






= 


0 


0 


... 0 




-Xn- 




flnp 1 


0 


0 


... 0 . 








- flm - 



(B.14b) 



(B.15) 



whence x can be computed by back-substitution. It is apparent, then, that 
the last m — n equations of the foregoing system are incompatible: their 
left-hand sides are zero, while their right-hand sides are not necessarily 
so. What the right-hand sides of these equations represent, then, is the 
approximation error in orthogonal coordinates. Its Euclidean norm is, then, 

lleoll ^ + + (B.16) 

The second method discussed here is based on the application of a chain 
of n reflections { Hj, }", known as Householder reflections, to both sides of 
eq.(B.l). The purpose of these reflections is, again, to obtain a represen- 
tation of matrix A in upper-triangular form (Golub and Van Loan, 1989). 
The algorithm proceeds as follows: We assume that we have applied re- 
flections Hi, H2, . . ., in this order, to A that have rendered it in 

upper-trapezoidal form, i.e., 

Ai 1 = Hi 1...H2H1A 



1“ + 


to 


••• 




^In 


0 


®22 


®2,i-l 


«2i 


*^2n 


0 


0 




«3i 


*^3n 


0 


0 


■ ■ ■ 






0 


0 


0 




+ 

%n 


. 0 


0 


0 




... 

mn 



The next Householder reflection, Hj, is determined so as to render the last 
m — i components of the *th column of HjAj_i equal to zero, while leaving 
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its first i — 1 columns unchanged. We do this by setting 



ai = sgn(ab)y^(a*J2 + + • • • + (a^.)^ 

Ui = [0 0 ••• 0 at + ai ■■■ a* 

T 

n - n 



where sgn(x) is defined as +1 if x > 0, as —1 if x < 0, and is left undefined 
when X = 0. As the reader can readily verify, 

2 II 

and hence, the denominator appearing in the expression for Hj is calculated 
with one single addition and a single multiplication. It is noteworthy that 
Hi, as defined above, is the n x n counterpart of the 3x3 pure reflection 
defined in eq.(2.5). As a matter of fact, Hj reflects vectors in m-dimensional 
space onto a hyperplane of unit normal Ui/||ui||. 

It is important to realize that 

(a) Oj is defined with the sign of a*^ because /3j is a multiple of the ith 
component of Uj, which is, in turn, the sum of a*^ and Oj, thereby 
guaranteeing that the absolute value of this sum will always be greater 
than the absolute value of each of its terms. If this provision were 
not made, then the resulting sum could be of a negligibly small ab- 
solute value, which would thus render /3j a very small positive num- 
ber, thereby introducing unnecessarily an inadmissibly large roundoff- 
error amplification upon dividing the product UjU^ by flp 

(b) an arbitrary vector v is transformed by Hj with unusually few flops, 
namely, 

HjV = V - ^(v^Ui)uj 
Hi 



Upon application of the n Householder reflections thus defined, the sys- 
tem at hand becomes 

HAx = Hb (B.18) 

with H defined as 

H = H„...H2Hi (B.19) 

Similar to that in equation (B.15), the matrix coefficient of x in eq.(B.18), 
i.e., HA, is in upper-tri angular form. That is, we have 



HA 



U 



Hb 



hu 

bi 



(B.20) 
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with Om'n denoting the (m — n) x n zero matrix, m' = m — n, and by 
and by being n- and m'-dimensional vectors. The unknown x can thus be 
calculated from eq.(B.18) by back-substitution. 

Note that the last m' components of the left-hand side of eq.(B.18) are 
zero, while the corresponding components of the right-hand side of the 
same equation are not necessarily so. This apparent contradiction can be 
resolved by recalling that the overdetermined system at hand in general has 
no solution. The lower part of b, by, is then nothing but an m'-dimensional 
array containing the nonzero components of the approximation error in the 
new coordinates. That is, the least-square error, Bq, in these coordinates 
takes the form 

eo = (B.21a) 



Therefore, 



(B.21b) 



B.2 The Underdetermined Case 

In this section we study the solution of system (B.l) under the assump- 
tion that m < n and rank(A) = m. Now, the system under study admits 
infinitely many solutions, which allows us to impose one condition on a 
specific solution that we may want to obtain. The obvious choice is a mini- 
mality condition on a norm of x. As in the previous section, the minimiza- 
tion of the square of the Euclidean norm of x leads to a linear problem, 
and hence, a direct solution of the problem at hand is possible. We thus 
have 

^(x) = ^l|x ||2 ^ min (B.22) 

subject to the constraint represented by eq.(B.f). Since we now have a 
constrained minimization problem, we proceed to its solution via Lagrange 
multipliers. That is, we introduce a new objective function C(x), defined as 



C(x) = z(x) + A^(Ax - b) 



(B.23) 



subject to no constraints, with A defined as an m-dimensional vector of 
Lagrange multipliers, as yet to be determined. We thus have now an un- 
constrained minimization problem with m + n design variables, the m com- 
ponents of A and the n components of x, that we group in the (m + n)- 
dimensional vector y = [x^ A^]^. The normality condition of the fore- 

going problem can now be stated as 



(B.24a) 
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with Om+n defined as the (m + n)-dimensional zero vector. The above 
condition can be broken down into the two conditions below: 



dx 

dX 



0 



n 



0 



m 



with Ojn and 0„ defined, respectively, as the m- and n-dimensional zero 
vectors. The above equations thus lead to 



■^ = X + A^A = 

ax 

■^ = Ax - b = 0™ 

dX 



(B.25) 

(B.26) 



Upon elimination of A from the above system of equations, we obtain 



X = A^(AA^)-^b 



(B.27) 



which is the rriinirnurn-norrn solution of the proposed problem. Again, the 
formula yielding the foregoing solution is deceptively simple. If we attempt 
the calculation of the inverse occurring in it, we risk introducing unneces- 
sarily an inadmissibly ill-conditioned matrix, the product AA^. Therefore, 
an alternative approach to the straightforward implementation of the above 
formula should be attempted, as we do in the subsection below. 



B.2.1 The Numerical Solution of an Underdetermined 
System of Linear Equations 



The simplest way of solving this problem is by introducing the m x m 
identity matrix 1 , in a disguised manner, between the two factors of the left- 
hand side of eq.(B.l). To this end, we assume that we have an orthogonal 
m X m matrix H, so that 

H^H = 1 (B.28) 

equation (B.l) thus becoming 

AH^Hx = b (B.29a) 

which can be rewritten in the form 

AH^v = b (B.29b) 

with V defined, obviously, as 

V = Hx (B.29c) 

Now, H is chosen as the product of m Householder reflections that trans- 
forms A^ into upper-triangular form, i.e., so that 



HA^ 



U 



(B.30) 
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with On'm defined as the (n — m)xm zero matrix and n' = n — m. Moreover, 
U is upper-triangular. Further, let us partition v in the form 



V = 



V(7 

Vl 



(B.31) 



Upon substitution of eqs.(B.30) and (B.31) into eq.(B.29b), we obtain 






V(7 

Vl 



where Omn' is the m x (n — m) zero matrix. Hence, 

U^V(7 + Omn'^L = b 



(B.32) 



whence it is apparent that vl can attain any value. Now, since v is the 
image of x under an orthogonal transformation, the Euclidean norms of 
these two vectors are identical, and hence, 

l|xf = ||v^f + ||vLf (B.33) 



Therefore, if we want to minimize the Euclidean norm of x, the obvious 
choice of Vl is zero. Furthermore, from eq.(B.32), 

wu = (B.34) 



and so. 



X = H^v 






U 

0 „' 



(B.35) 



with 0„/ denoting the n'-dimensional zero vector, thereby completing the 
numerical solution of the problem at hand. 




Exercises 



While the following exercises are ordered by chapter, the ordering within 
each chapter does not necessarily correspond to that of the sections within 
the chapter. Some of the exercises call for algebraic manipulations that are 
cumbersome and error- prone if done by hand. It is strongly recommended 
that these exercises be worked out using software for computer algebra, 
which is nowadays readily available (Pattee, 1995). On the other hand, 
some problems require numerical computations that most of the time can be 
done by longhand calculations; when these become cumbersome, the reader 
is advised to resort to suitable software, e.g., Matlab and its toolboxes 
(Hanselman and Littlefield, 2001). 



1 An Overview of Robotic Mechanical Systems 

The exercises listed below are meant to familiarize the uninitiated reader 
with the issues involved in robotics, especially in the area of robotic me- 
chanical systems. A major issue, regrettably very often overlooked, is the 
terminology. In attempting to work out these exercises, the beginner should 
be able to better understand the language of robotics and realize that a 
common terminology is not yet available. 

1.1 List some definitions of machine, say about half a dozen, trying to 
cover the broadest timespan to date. Hint: Hartenberg and Denavit 
( 1 964 ) o, few bibliographical references. 
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1.2 Try to give an answer to the question: Are intelligent machines possi- 
ble ? Express your own ideas and explore what scientists like Penrose 
(1994) think about this controversial issue. 

1.3 What is the difference among machine, mechanism, and linkage! In 
particular, analyze critically the definitions given by authorities, such 
as those found in the most respected dictionaries, encyclopedias, and 
archival documents of learned societies, e.g., the complete issue of 
Vol. 26, No. 5 (1991) of Mechanism and Machine Theory on termi- 
nology. 

1.4 What is artificial intelligence? What is fuzzy logic? Can the tech- 
niques of these fields be applied to robotics? 

1.5 What is mechatronics? What is the difference between mechatronics 
and robotics? Comerford (1994) and Soureshi et al. (1994) give an 
account on this technology. 

1.6 What do you understand as dexterity! The concept of dexterity is 
normally applied to persons. Can it be applied to animals as well? 
What about machines? 

1.7 Define the term algorithm. In this context, make a clear distinction 
between recursion and iteration. Note that in the robotics literature, 
there is often confusion between these two terms in particular. Make 
sure that you do not make the same mistake! Again, Penrose (1994) 
has provided an extensive discussion on the nature of algorithms. 

1.8 What is the difference among terms like real-time, on-line, and run- 
time! 

1.9 How fast can two floating-point numbers be multiplied using a per- 
sonal computer? What about using a UNIX workstation? a supercom- 
puter? Write a piece of code to estimate this time on your computer 
facility. 

1.10 Answer the foregoing question as pertaining to floating-point addi- 
tion. 

1.11 What is the smallest floating-point number on your computer? Rather 
than looking for the answer in manuals, write a procedure to estimate 
it. 

1.12 What is the difference between conventional programming and object- 
oriented programming! In terms of programming languages, what is 
the difference between C and C++? Rumbaugh et al. (1991) provide 
an introduction to object-oriented programming, while Stroustrup 
(1991) gives an introduction to C++. 
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2 Mathematical Background 

2.1 Prove that the range and the nullspace of any linear transformation 
L of vector space U into vector space V are vector spaces as well, the 
former of V, the latter of lA. 

2.2 Let L map U into V and dim{W} = n, dim{V} = m. Moreover, let TZ 
and AT be the range and the nullspace of L, their dimensions being p 
and i/, respectively. Show that pA-v = n. 

2.3 Given two arbitrary nonzero vectors u and v in f®, find the matrix 
P representing the projection of onto the subspace spanned by u 
and V. 

2.4 Verify that P, whose matrix representation in a certain coordinate 
system is given below, is a projection. Then, describe it geometri- 
cally, i.e., identify the plane onto which the projection takes place. 
Moreover, find the nullspace of P. 



[P] 




1 

2 

1 



-1 

1 

2 



2.5 If for any 3-dimensional vectors a and v, matrix A is defined as 

. disL X v) 

then we have 

. rp d(v X a) 

^ ^ 

av 

Show that A is skew-symmetric without introducing components. 

2.6 Let u and v be any 3-dimensional vectors, and define T as 

T = 1 + uv^ 

The (unit) eigenvectors of T are denoted by wi, W2, and W3. Show 
that, say, wi and W2 are any unit vectors perpendicular to v and 
different from each other, whereas W 3 = u/||u||. Also show that the 
corresponding eigenvalues, denoted by Ai, A2, and A3, associated with 
wi, W2, and W3, respectively, are given as 

Ai = A 2 = 1, A 3 = 1 + u • V 
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2.7 Show that if u and v are any 3-dimensional vectors, then 

det(l -h uv^) = 1 -h u • V 
Hint: Use the results of the Exercise 2.6. 

2.8 For the two unit vectors e and f in 3-dimensional space, define the 
two reflections 

Ri = 1 - 2ee^, R 2 = 1 - 2f 

Now, show that Q = R 1 R 2 is a rigid-body rotation, and find its axis 
and its angle of rotation in terms of unit vectors e and f. Again, no 
components are permitted in this exercise. 

2.9 State the conditions on the unit vectors e and f, of two reflections Ri 
and R 2 , respectively, under which a given rotation Q can be factored 
into the reflections Ri and R 2 given in the foregoing exercise. In 
other words, not every rotation matrix Q can be factored into those 
two reflections, for fixed e and f, but special cases can. Identify these 
cases. 

2.10 Prove that the eigenvalues of the cross-product matrix of the unit 
vector e are 0, j, and — j, where j = a/— 1- Find the corresponding 
eigenvectors. 

2.11 Without resorting to components, prove that the eigenvalues of a 

proper orthogonal matrix Q are 1, and with <f> denoting 

the angle of rotation. Hint: Use the result of the foregoing exercise 
and the Cay ley- Hamilton Theorem. 

2.12 Find the axis and the angle of rotation of the proper orthogonal 
matrix Q given below in a certain coordinate frame T . 

-2 2 ■ 

-1 -2 
-2 -1 

2.13 Find E and f of the exponential representation of the rotation matrix 
given in Exercise 2.12. 

2.14 Cayley’s Theorem, which is not to be confused with the Theorem of 
Cayley-Hamilton, states that every 3x3 proper orthogonal matrix 
Q can be uniquely factored as 

Q = (l-S)(l + S)-i 

where S is a skew-s}mimetric matrix. Find a general expression for S 
in terms of Q, and state the condition under which this factoring is 
not possible. 



[Qe] 



-1 

-2 

2 
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2.15 Find matrix S of Cayley’s factoring for Q as given in Exercise 2.12. 

2.16 If Q represents a rotation about an axis parallel to the unit vector e 
through an angle </>, then the Rodrigues vector p of this rotation can 
be defined as 




Note that if r and tq denote the Euler-Rodrigues parameters of the 
rotation under study, then p = r/rg. Show that 

p = — vect(S) 

for S given in Exercise 2.14. 

2.17 The vertices of a cube, labeled A, B, . . ., H, are located so that A, 
B, C, and D, as well as E, F, G, and H, are in clockwise order when 
viewed from outside. Moreover, AE, BEL, CG, and DF are edges of 
the cube, which is to be manipulated so that a mapping of vertices 
takes place as indicated below: 

A-^ D, B^G, G ^G, D^F 
E^ A, F ^ E, G-^H, H -^B 

Find the angle of rotation and the angles that the axis of rotation 
makes with edges AB, AD, and AE. 

2.18 (Euler angles) A rigid body can attain an arbitrary configuration 
starting from any reference configuration, 0, by means of the compo- 
sition of three rotations about coordinate axes, as described below: 
Attach axes Xq, Yq, and Zq to the body in the reference configuration 
and rotate the body through an angle <f> about Zq, thus carrying the 
axes into Xi, Yi, and Z\ (=Zo), respectively. Next, rotate the body 
through an angle 0 about axis Y\, thus carrying the axes into X 2 , 
Y 2 , and Z 2 , respectively. Finally, rotate the body through an angle -0 
about Z 2 so that the axes coincide with their desired final orientation, 
X3, I3, and Y3. Angle -0 is chosen so that axis Y3 lies in the plane of 
Zq and Xi, whereas angle 0 is chosen so as to carry axis Z\ (=Zo) 
into Z 3 (=^2). Show that the rotation matrix carrying the body from 
configuration 0 to configuration 3 is: 

c9c(j)cil) — s(j)sil) —c9c4>siIj — s4>ci^ s9c4> 

Q = c9s4>ciIj + c4>s'i^ —c9s(ps'tp + c<j)cil) s9s<p 
— s9cip s9sip c9 

where c(-) and s(-) stand for cos(-) and sin(-), respectively. Moreover, 
show that a, the angle of rotation of Q given above, obeys the relation 
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2.19 Given an arbitrary rigid-body rotation about an axis parallel to the 
unit vector e through an angle </>, it is possible to find both e and <f> 
using the linear invariants of the rotation matrix, as long as the vector 
invariant does not vanish. The latter happens if and only if </> = 0 or 
7T. Now, if (/) = 0, the associated rotation matrix is the identity, and 
e is any 3-dimensional vector; if <f> = n, then we have 

Q(7t) = Qjt = -1 + 2ee^ 

whence we can solve for ee^ as 

+ 1 ) 

Now, it is apparent that the three eigenvalues of Qtt are real and the 
associated eigenvectors are mutually orthogonal. Find these. 

2.20 Explain why all the off-diagonal entries of a symmetric rotation ma- 
trix cannot be negative. 

2.21 The three entries above the diagonal of a 3 x 3 matrix Q that is 
supposed to represent a rotation are given below: 

_ 1 _ 2 _ 3 

'112 — qiZ — '?23 — ^ 

Without knowing the other entries, explain why the above entries are 
unacceptable. 

2.22 Let pi, p 2 , and pa be the position vectors of three arbitrary points 
in 3-dimensional space. Now, define a matrix P as 

P=[pi p2 Pa] 

Show that P is not frame- invariant. Hint: Show, for example, that it 
is always possible to find a coordinate frame in which tr(P) vanishes. 

2.23 For P defined as in Exercise 2.22, let 

q = tr(P^) — tr^(P) 

Show that q vanishes if the three given points and the origin are 
collinear, for P represented in any coordinate frame. 

2.24 For P defined, again, as in Exercise 2.22, show that PP^ is invari- 
ant under frame-rotations about the origin, and becomes singular if 
and only if either the three given points are collinear or the origin 
lies in the plane of the three points. Note that this matrix is more 
singularity-robust than P. 
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(b) 



FIGURE 1. A cube in two different configurations. 



2.25 The diagonal entries of a rotation matrix are known to be —0.5, 0.25, 
and —0.75. Find the off-diagonal entries. 

2.26 As a generalization to the foregoing exercise, discuss how you would 
go about finding the off-diagonal entries of a rotation matrix whose 
diagonal entries are known to be a, 6, and c. Hint: This problem can he 
formulated as finding the intersection of the coupler curve of a four- 
bar spherical linkage (Chiang, 1988), which is a curve on a sphere, 
with a certain parallel of the same sphere. 

2.27 Shown in Fig. 1(a) is a cube that is to be displaced in an assembly 
operation to a configuration in which face EFGH lies in the XY 
plane, as indicated in Fig. 1(b). Compute the unit vector e parallel 
to the axis of the rotation involved and the angle of rotation for 
0 < (p < t:. 

2.28 The axes Xi, Yi, Z\ of a frame T\ are attached to the base of a 
robotic manipulator, whereas the axes X 2 , Y 2 , -^2 of a second frame 
T 2 are attached to its end-effector, as shown in Fig. 2. Moreover, the 
origin P of P 2 has the Yi-coordinates (1, —1, 1). Furthermore, the 
orientation of the end effector with respect to the base is defined by 
a rotation Q, whose representation in Pi is given by 

1 I-a/3 I + a/3' 

1 + a/3 1 I-a/3 

I-a/3 I + a/3 1 

(a) What are the end-effector coordinates of point C of Fig. 2? 

(b) The end-effector is approaching the ABC plane shown in Fig. 2. 
What is the equation of the plane in end-effector coordinates? 
Verify your result by substituting the answer to (a) into this 
equation. 
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2.29 Shown in Fig. 3 is a cube of unit side, which is composed of two 
parts. Frames (Xq, Foj ^o) and (Xi, Yi, Zi) are attached to each of 
the two parts, as illustrated in the figure. The second part is going 
to be picked up by a robotic gripper as the part is transported on a 
belt conveyor and passes close to the stationary first part. Moreover, 
the robot is to assemble the cube by placing the second part onto the 
first one in such a way that vertices Ai, Bi, Ci are coincident with 
vertices Xq, Bq, Cq. Determine the axis and the angle of rotation that 
will carry the second part onto the first one as described above. 

2.30 A piece of code meant to produce the entries of rotation matrices is 
being tested. In one run, the code produced a matrix with diagonal 
entries —0.866, —0.866, —0.866. Explain how without looking at the 
other entries, you can decide that the code has a bug. 




FIGURE 3. Roboticized assembly operation. 
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FIGURE 4. Three configurations of a cube. 

2.31 Shown in Fig. 4 is a rigid cube of unit side in three configurations. The 
second and the third configurations are to be regarded as images of 
the first one. One of the last two configurations is a reflection, and the 
other is a rotation of the first one. Identify the rotated configuration 
and And its associated invariants. 

2.32 Two frames, Q and C, are attached to a robotic gripper and to a 
camera mounted on the gripper, respectively. Moreover, the camera 
is rigidly attached to the gripper, and hence, the orientation of C with 
respect to (/, denoted by Q, remains constant under gripper motions. 
The orientation of the gripper with respect to a frame B fixed to 
the base of the robot was measured in both Q and C. Note that this 
orientation is measured in Q simply by reading the joint encoders, 
which report values of the joint variables, as discussed in detail in 
Chapter 4. The same orientation is measured in C from estimations 
of the coordinates of a set of points fixed to B, as seen by the camera. 
Two measurements of the above-mentioned orientation, denoted Ri 
and R 2 , were taken in Q and C, with the numerical values reported 
below: 







0.667 


0.333 


0.667' 


Ri 


]g = 


-0.667 


0.667 


0.333 






-0.333 


-0.667 


0.667 






'0.500 


0 


0.866' 




Ri 


]c = 


0 1 


.000 


0 


7 






0.866 


0 0.500 








0.707 


0.577 


0.408 


1 


R 2 


h = 


0 


0.577 


-0.816 




-0.707 


0.577 


0.408 


J 






'1 0 


0 


1 






R 2 


]c = 


0 0.346 


-0.938 










0 0.938 


0.346 







(a) Verify that the foregoing matrices represent rotations. 

(b) Verify that the first two matrices represent, in fact, the same 
rotation Ri, albeit in different coordinate frames. 
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(c) Repeat item (b) for R 2 . 

(d) Find [Q]c;. Is your computed Q orthogonal? If not, what is the 
error in the computations? Note that you may have encountered 
here a problem of roundoff error amplification, which can be 
avoided if a robust computational scheme is used. As a matter 
of fact, a robust method in this case can be devised by resorting 
to the Gram-Schmidt orthogonalization procedure, as outlined in 
Appendix B. 

2.33 The rotation Q taking a coordinate frame B, fixed to the base of a 
robot, into a coordinate frame Q, fixed to its gripper, and the position 
vector g of the origin of Q have the representations in B given below: 



Q]b = q 


1 I-a/3 I + a/3' 




'i-a/3' 


1 + a/3 1 I-a/3 


, [s]b = 


V3 


3 


I-a/3 I + a/3 1 




_1 + V3_ 



Moreover, let p be the position vector of any point V of the 3- 
dimensional space, its coordinates in B being (x, y, z). The robot is 
supported by a cylindrical column C of circular cross section, bounded 
by planes II ^ and II^. These are given below: 

C: = 4 . 77 ^. z = 0] il 2 : z = 10 

Find the foregoing equations in Q coordinates. 

2.34 A certain point of the gripper of a robot is to trace an elliptical path 
of semiaxes a and 6 , with center at C, the centroid of triangle PQR, 
as shown in Fig. 5. Moreover, the semiaxis of length a is parallel to 
edge PQ, while the ellipse lies in the plane of the triangle, and all 
three vertices are located a unit distance away from the origin. 

(a) For b = 2a/3, the gripper is to keep a fixed orientation with 
respect to the unit tangent, normal, and binormal vectors of the 
ellipse, denoted by ej, e„, and 65 , respectively^. Determine the 
matrix representing the rotation undergone by the gripper from 
an orientation in which vector ej is parallel to the coordinate axis 
X, while e„ is parallel to Y and Bf, to Z . Express this matrix in 
X, y, Z coordinates, if the equation of the ellipse, in parametric 
form, is given as 

x' = a cosy, y' = 6 siny, z' = 0 

the orientation of the gripper thus becoming a function of y. 

(b) Find the value of y for which the angle of rotation of the gripper, 
with respect to the coordinate axes X, Y, Z, becomes tt. 



^An account of curve geometry is given in Section 9.2 
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2.35 With reference to Exercise 2.27, find Euler angles </>, 9, and 'tp that 
will rotate the cube of Fig. la into the attitude displayed in Fig. lb. 
For a definition of Euler angles, see Exercise 2.18 

2.36 Find a sequence of Euler angles <f>, 9, and 'ip, as defined in Exer- 
cise 2.18, that will carry triangle Ai, Bi, Ci into triangle Mq, Bq, Cq, 
of Fig. 3. 

3 Fundamentals of Rigid-Body Mechanics 

3.1 The cube of Fig. 6 is displaced from configuration AB . . . H into con- 
figuration A'B' . . .H' . 

(a) Determine the matrix representing the rotation Q undergone by 
the cube, in X, Y, Z coordinates. 

(b) Find the Pliicker coordinates of line C of the cube undergoing 
displacements of minimum magnitude. 

(c) Find the intersections of C with the coordinate planes. 

3.2 Two unit forces, fi and f2, are applied to the regular tetrahedron of 
unit-length edges displayed in Fig. 7 in such a way that fi is directed 
from P 2 to P3, whereas f2 is directed from P 4 to Pi. The effect of 
the foregoing system of forces on the rigid tetrahedron is obtained 
by application of the resultant of the two forces on a certain point 
P and a moment n. Find the location of point P lying closest to P 4 
that will make the magnitude of n a minimum. 
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3.3 The moment of a line £i about a second line £2 is a scalar defined 
as 



= 111 • 62 



where rii is the moment of £1 about an arbitrary point P of £ 2 , while 
62 is a unit vector parallel to line £ 2 . Apparently, the necessary and 
sufficient condition for two lines to intersect is that the moment of 
one about the other vanish. 



Using the above concept, show that the locus of all lines £ intersecting 
three given lines { £fc is a quadric surface, i.e., a surface defined by 
a function that is quadratic in the position vector of a point of the 
surface. Hint: Note that the moment of any line o/{ C}~ with respect 
to £ vanishes. 



Note; The quadric surface above is, in fact, a ruled surface, namely, 
a one-sheet hyperboloid. 



3.4 A robotic gripper is provided with two redundant sensors that are 
meant to measure a wrench acting on the gripper. The *th sensor, 
moreover, has its own coordinate frame, labeled Pj, for i = 1 , 2 . 
Sensor i reported the *th measurement of the wrench wp, where 
subscript P indicates that the force is applied at point P, as [wp ]j = 
[n^, f^]f, for * = 1,2. These measurements are given as 



n]i = 


1 

0 0 
1 


, [f]i = 


1 

0 CM 

1 




5 




1 

0 

1 





■ -5/3 ■ 




■-4/3' 


n]2 = 


-10/3 


, [f]2 = 


4/3 




10/3 




2/3 



(a) Show that the measurements are compatible, based on invari- 
ance arguments. 

(b) Determine the relative orientation of the two frames, i.e., find the 
rotation matrix transforming p 2 -coordinates into Pi-coordinates. 

3.5 A robot-calibration method has been proposed that allows us to de- 
termine the location of a joint axis, £, via the Pliicker coordinates 
of the axis in a coordinate frame fixed to the gripper. The Pliicker 
coordinates are given as TTp = [e^, n^]^. 

(a) Show that the distance of the axis to the origin of the gripper- 
fixed coordinate frame, d, can be determined as d = ||n||. 

(b) Show that the point P* on the axis, which lies closest to the 
above-mentioned origin, has a position vector p* given as 



p = e X n 




468 



Exercises 



(c) From measurements on a robot, the Pliicker coordinates were 
estimated, in a gripper-fixed frame Q, as 

[ 7 T£]e = [-V2/2, 0, V2/2, 0, -V2, op 

Find d and p* in gripper coordinates 

3.6 The gripper Q of a robot is approaching a workpiece B, as indicated 
in Fig. 8, with planes ili and II 2 parallel to each other and normal to 
Ids- The workpiece is made out of a cube of unit length from which 
two vertices have been removed, thereby producing the equilateral 
triangular faces DEF and D'E'F' . Moreover, two coordinate frames, 
T (X, Y, Z) and T' (X', Z'), are defined as indicated in the 

figure, in which Y is, apparently, parallel to line D'C . 

It is required to grasp B with Q in such a way that planes ili and II 2 
coincide with the triangular faces, while carrying the Y' axis to an 
orientation perpendicular to the diagonal CC of B. More concretely, 
in the grasping configuration, frame T' is carried into E" (X", X", 
and X"), not shown in the figure, in such a way that unit vectors i", 
j", k", parallel to X", X", X", respectively, are oriented so that i" has 
all three of its X-components positive, while j" has its X-component 
positive. 



(a) Compute the angle of rotation of the motion undergone by Q 
from a pose in which T' and T have identical orientations, 
termed the reference pose, and find the unit vector parallel to 
the axis of rotation, in frame X. 



(b) 



The position vector of point P of ^ is known to be, in the ref- 
erence pose, 



[p]e 



2 

-1 

0.25 




FIGURE 8. A workpiece B to be grasped by a gripper Q. 
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Determine the set of points of Q undergoing a displacement of 
minimum magnitude, under the condition that P, in the dis- 
placed configuration of Q, coincides with C . 

3.7 In calibrating a robot, the Pliicker coordinates of one of its axes are to 
be determined in a given coordinate frame. To this end, the moment 
of this axis is measured with respect to two points, A and B, of 
position vectors [a] = [1, 0, 0]^ and [b] = [0, 1, 1]^, respectively. 
The said moments, ua and n^, respectively, are measured as 





'o' 




'o' 


Ha] = 


2 

0 


, [ns] = 


1 

1 



with all entries given in meters. 

(a) Determine the unit vector e defining the direction of the axis 
under discussion. 

(b) Find the coordinates of the point P* of the axis that lies closest 
to the origin 

(c) Find the Pliicker coordinates of the axis about the origin, i.e., the 
Pliicker coordinates of the axis in which the moment is defined 
with respect to the origin. 

3.8 Prove that for any 3-dimensional vectors uj and p, 

W X (w X • • • (w X (w xp)) • • •) = (-l)'^(llwf - ||wf 

" V " 

2k factors 

U) X {u> X ■ ■ ■ {u> X {u> xp)) • • •) = X P 

' V " 

2k-\-l factors 

3.9 A “small” rotation is defined as that about an arbitrary axis parallel 
to the unit vector e, through a “small” angle <f>, so that <f> << 1. 
Prove that the angular- velocity vector, in the special case of “small” 
rotations, turns out to be a time-derivative. What is the vector whose 
time-derivative yields the angular- velocity vector? 

3.10 Derive an expression for the angular velocity uj in terms of Euler 
angles, which were introduced in Exercise 2.18. More specifically, if 
we store the Euler angles in array rj = [0, <f>, then, find the 

matrix W such that 

U! = Wf/ 

Notice that, given rj and tu, an expression for rj can be obtained upon 
inverting W. However, W is not always invertible. Find under which 
conditions W becomes singular. Notice: The use of computer algebra 
is strongly recommended to solve this exercise. 
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3.11 A rectangular prism with regular hexagonal bases whose sides are 
25 mm long and whose height is 150 mm is to undergo a pick-and- 
place operation — See Chapter 5 to understand what this means — that 
requires knowledge of its centroid location and its moment-of-inertia 
matrix. Find the centroidal principal axes and moments of inertia 
under the assumption that the prism is made from a homogeneous 
material. 

3.12 The prism of Exercise 3.11 now undergoes a machining process cut- 
ting it into two parts, which are separated by a plane that contains 
one of the edges of the base and makes an angle of 45° with the axis of 
the prism. Find the centroidal principal axes and moments of inertia 
of each of the two parts. 

3.13 In Exercise 2.22 assume that a mass m is located at every point Pi 
of position vector p* . Give a mechanical interpretation of the matrix 
m[tr(PP^)l — PP^], with P defined in that exercise. 

3.14 The centroidal inertia matrix of a rigid body is measured by two 
observers, who report the two results below: 

'1 0 0] 1 r® 2 

[I]^= 0 2 0, [I]b = - 2 5 0 

0 0 3j *^[2 0 7 

Show that the two measurements are acceptable. Hint: Use invariance 
arguments. 

3.15 State the conditions under which a point and the mass center of a 
rigid body share the same principal axes of inertia. In other words, let 
Ip and Ic be the moment-of-inertia matrices of a rigid body about 
a point P and its mass center, C, respectively. State the conditions 
under which the two matrices have common eigenvectors. Moreover, 
under these conditions, what are the relationships between the two 
sets of principal moments of inertia? 

3.16 Show that the smallest principal moment of inertia of a rigid body 
attains its minimum value at the mass center. 

3.17 Show that the time-rate of change of the inertia dyad M of a rigid 
body is given by 

M = WM - MW 

Then, recalling the momentum screw p defined as 

fx = Mt 

where t is the twist of the body, defined at its mass center. Now, with 
the above expression for M, restate the result displayed in eq.(3.148), 
i.e., show that 



p = Mt + WMt 
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3.18 A wrench w = [n^ f^]^, with f acting at point P of the gripper of 
Fig. 2, is measured by a six-axis force sensor, to which a frame Ps is 
attached, as indicated in that figure. If points P and S he a distance 
of 100 mm apart, find the wrench in P 2 , when the readouts of the 
sensor are 

'll [O' 

[n]s= 0 Nm, [f]s= 1 N 

1 0 



4 Kinetostatics of Simple Robotic Manipulators 

Exercises 4.22 to 4.27 below pertain to Section 4.9. They are thus 
to be assigned only if this section was covered either in class or 
as a reading assignment. 

4.1 Shown in Fig. 8.8 is the kinematic chain of one of the six-dof legs 
of a flight simulator, such as that appearing in Fig. 1.5. The HD 
parameters of this manipulator are displayed in Table 8.6. In that 
figure, A4 is the moving platform to which an aircraft cockpit is rigidly 
attached. The six-dof motion of A4 is controlled by means of the 
hydraulic cylinder indicated in the same figure as a prismatic pair. 
Find all inverse kinematics solutions of this manipulator, relating the 
pose of A4 with all the joint variables. 

4.2 Modify the inverse-kinematics solution procedure of Section 4.3 to 
obtain all the postures of a PRR manipulator that give the same 
EE pose, and show that this problem leads to a quartic polynomial 
equation. 

4.3 Repeat Exercise 4.2 as pertaining to a PRP manipulator. 

4.4 The manipulator appearing in Fig. 9 is of the orthogonal type, with a 
decoupled, spherical wrist, and a regional structure consisting of two 
parallel axes and one axis perpendicular to these two. In that figure, 
rectangles denote revolutes of axes l}4ng in the Xi-Zi plane, while 
circles with dots indicate revolutes with axes normal to the plane of 
the figure. Find all inverse kinematics solutions for arbitrary poses of 
the EE of this manipulator. 

4.5 Similar to the manipulator of Fig. 9, that of Fig. 10 is of the orthog- 
onal, decoupled type, except that the latter has a prismatic pair. For 
an arbitrary pose of its EE, And all inverse kinematics solutions of 
this manipulator. For a description of the meaning of the rectangles 
and the circles with dots inside, see Exercise 4.4. 



4.6 Derive expressions for the angle of rotation and the unit vector paral- 
lel to the axis of rotation of matrices Qj, as introduced in Section 4.2. 
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FIGURE 9. A six-revolute robot holding a heavy tool. 




FIGURE 10. ABB-IRB 1000 robotic manipulator. 

4.7 The robotic manipulator of Fig. 9 is instrumented with sensors mea- 
suring the torque applied by the motors at the joints. Two readouts 
are taken of the sensors for the robot in the configuration indicated 
in the figure. In the first readout, the gripper is empty; in the second, 
it holds a tool. If the first readout is subtracted from the second, the 
vector difference At is obtained as 

At = [0 2 1 0 1 0]^ Nm 

With the foregoing information, determine the weight w of the tool 
and the distance d of its mass center from C, the center of the spher- 
ical wrist. For a description of the meaning of the rectangles and the 

circles with dots inside, see Exercise 4.4. 

4.8 A planar three-axis manipulator is shown in Fig. 11, with a\ = a 2 = 
U 3 = 1 m. When a wrench acts onto the EE of this manipulator, the 
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P(x,y) 




joint motors exert torques that keep the manipulator under static 
equilibrium. Readouts of these joint torques are recorded when the 
manipulator is in the posture 0\ = O 2 = 0^^ = 45°, namely, 

Ti = — %/2 Nm, T 2 = — %/2 Nm, T 3 = 1 — %/2 Nm 

Calculate the above-mentioned wrench. 

4.9 Shown in Fig. 12 is a computer-generated model of DIESTRO, the robot 
displayed in Fig. 4.31, with a slightly modified EE. The Denavit-Flartenberg 
parameters of this robot are given in Table 1. Find the Jacobian matrix of 
the manipulator at the above configuration. 

4.10 An orthogonal spherical wrist has the architecture shown in Fig. 4.18, with 
the DFl parameters 

04 = 90°, 05 = 90° 

A frame T’j is attached to its EE so that coincides with Z^. Find the 
(Cartesian) orientation that can be attained with two inverse kinematics 

TABLE 1. DH parameters of the modified DIESTRO 



% 


tti (mm) 


bi (mm) 


CXi 


Oi 


1 


50 


50 


90° 


90° 


2 


50 


50 


-90° 


-90° 


3 


50 


50 


90° 


90° 


4 


50 


50 


-90° 


-90° 


5 


50 


50 


90° 


90° 


6 


0 


50 


-90° 


-90° 
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solutions 01 and 0u, defining the two distinct postures, that lie the far- 
thest apart. Note that a distance between two manipulator postures can be 
defined as the radical of the quadratic equation yielding the two inverse 
kinematic solutions of the wrist, whenever the radical is positive. Those 
postures giving the same EE orientation and lying farthest from each other 
are thus at the other end of the spectrum from singularities, where the two 
postures merge into a single one. Hence, the postures lying farthest from 
each other are singularity-robust. 

4.11 For the two postures found in Exercise 4.10, the EE is to move with an 

angular velocity uj = [uji, uj 2 , Show that if ||tu|| remains constant, 

then so does ||0||, for 9 defined as the joint-rate vector of the wrist. 

4.12 Point C of the manipulator of Fig. 4.15 is to move with a velocity v in the 
posture displayed in that figure. Show that as long as ||v|| remains constant, 
so does ||0||, for 9 defined as the joint-rate vector. Moreover, let us assume 
that in the same posture, point C is to attain a given acceleration a. In 
general, however, ||0||, where 9 denotes the corresponding joint-acceleration 
vector, does not necessarily remain constant under a constant ||a||. Under 
which conditions does ||a|| remain constant for a constant ||0||? 

4.13 A load f is applied to the manipulator of Fig. 4.15 in the posture displayed 
in that figure. Torque cells at the joints are calibrated to supply torque 
readouts resulting from this load only, and not from the dead load — its 
own weight — of the manipulator. Show that under a constant-magnitude 
load, the magnitude of the joint-torque vector remains constant as well. 



u 




FIGURE 12. A six-revolute manipulator. 
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4.14 Dialytic elimination. The characteristic polynomial of decoupled manip- 
ulators for positioning tasks can be derived alternatively via dialytic elim- 
ination, as introduced in Subsection 4.5.3. It is recalled here that dialytic 
elimination consists in eliminating one unknown from a system of poly- 
nomial equations by expressing this system in linear homogeneous form, 
whereby each equation is a linear combination of various successive powers 
of the unknown to be eliminated, including the zeroth power. This elimina- 
tion can be achieved as outlined below: In eqs.(4.19a) and (4.20a), express 
COS01 and sin^i in terms of tan(<?i/2) = t\, thereby obtaining 

{C c^ -\- D s^ -\- E — A) t\ “2 Bti + (C c^ D E A) = 0 
{H C3 + / S3 + T — E) + 2 G ti + [E[ C3 + / S3 + T + E) = 0 

which can be further expressed as 

mt^+2Bti + n = 0 
pt\ E 2Gt\ E q = 0 

with obvious definitions for coefficients m, n, p, and q. Next, both sides of 
the two foregoing equations are multiplied by ti, thereby producing 

mtf E 2 B tf E nti = 0 
pitf E 2 G t\ E qti = 0 

Now, the last four equations can be regarded as a system of linear homo- 
geneous equations, namely, 

Mti = 0 

where 0 is the 4-dimensional zero vector, while M is a 4 x 4 matrix, and 
ti is a 4-dimensional vector. These are defined as 

0 m 2B n" 

0 p 2G q 
m 2B n 0 ’ 

p 2G q 0. 

Clearly, ti 7^ 0, and hence, M must be singular. The characteristic poly- 
nomial sought can then be derived from the condition 

det(M) = 0 

Show that the last equation is quadratic in cos<?3 and sin<?3. Hence, the 
foregoing equation should lead to a quartic equation in tan(<?3/2). Derive 
the quartic equation involved. Elint: Do not do this by hand, for it may 
he too time-consuming and could quickly lead to algebraic mistakes. Use 
software for symbolic computations instead. 
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4.15 Given an arbitrary three-revolute manipulator, as shown in Fig. 4.9, its sin- 
gular postures are characterized by the existence of a line passing through 
its operation point about which the moments of its three axes vanish — see 
Exercise 3.3. Note that this condition can be readily applied to manipu- 
lators with a simple architecture, whereby two successive axes intersect at 
right angles and two others are parallel. However, more complex architec- 
tures, like that of the manipulator of Fig. 4.13, are more elusive in this 
regard. Find the line passing through the operation point and intersecting 
the three axes of the manipulator of Fig. 4.13 at a singularity. Hint: A 
singular posture of this manipulator urns found in Example 

4.16 A robot of the Puma type has the architecture displayed in Fig. 4.3, with 
the numerical values U2 = 0.432 m, 03 = 0.020 m, 63 = 0.149 m, 64 = 
0.432 m. Find its maximum reach R as well as the link length a of the 
manipulator of Fig. 4.15 with the same reach R. 

4.17 Compute the workspace volume of the manipulator of Fig. 4.3. Here, you 
can exploit the axial symmetry of the workspace by recalling the Pappus- 
Guldinus Theorems — see any book on multivariable calculus — that yield 
the volume as 27T(/, with q defined as the first moment of the cross-section, 
which is displayed in Fig. 4.22b, with respect to the axis of symmetry, Z\. 
To this end, you will need the first moment of a semicircle with respect to its 
diameter. This information is tabulated in books on elementary mechanics 
or multivariable calculus, a.k.a. advanced calculus. 

4.18 Compute the workspace volume of the manipulator of Fig. 4.15, whose 
workspace is sketched in Fig. 4.23. Here, you can also use the Pappus- 
Guldinus Theorem, as suggested in Exercise 4.17. However, the first mo- 
ment of the cross-section has to be determined numerically, for the area 
properties of the curve that generates the 3-dimensional workspace are not 
tabulated. Now, for two manipulators, the Puma-type and the one un- 
der discussion, with the same reach, determine which one has the larger 
workspace. Note: This exercise is not more difficult than others, hut it re- 
quires the use of suitable software for the calculation of area properties of 
planar regions hounded hy arbitrary curves. Unless you have access to such 
software, do not attempt this exercise. 

4.19 Shown in Fig. 10 is the kinematic chain of the ABB-IRB 1000 robotic ma- 
nipulator, which contains five revolutes and one prismatic pair. A revolute 
is represented either as a rectangle or as a circle, depending on whether its 
axis lies in the plane of the figure or is perpendicular to it. The prismatic 
pair is represented, in turn, as a dashpot. 

(a) Determine the manipulator Jacobian in the X\, Y\, Z\ coordi- 
nate frame shown in the figure. 
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(b) Determine the twist of the end-effector, defined in terms of the 
velocity of point P, for unit values of all joint-rates, and the 
posture displayed in the same figure. 

(c) Determine the joint accelerations that will produce a vanishing 
acceleration of the point of intersection, C, of the three wrist 
axes and a vanishing angular acceleration of the gripper, for the 
unit joint rates given before. 

4.20 The robot in Fig. 10 is now used for a deburring task. When the robot is 
in the configuration shown in that figure, a static force f and no moment 
acts on point P of the deburring tool. This force is sensed by torque sensors 
placed at the joints of the robot. Assume that the readings of the arm joints 
are ti = 0, T 2 = 100 Nm, and T 3 = 50 Nm. 

(a) Find the force f acting at P. 

(b) Find the readings of the torque sensors placed at the wrist joints. 

4.21 A decoupled manipulator is shown in Fig. 8.8 with the DFl parameters of 
Table 8.6 in an arbitrary posture. 

(a) Find the Jacobian matrix of this manipulator at a posture with 
axis Xi vertical and pointing downwards, while Z 2 and Y\ make 
an angle of 180°. Moreover, in this particular posture, and 
Z 4 are vertical and pointing upwards, while Z 7 makes an angle 
of 180° with Yi. 

(b) At the posture described in item (a), compute the joint-rates 
that will produce the twist 



w]l = 


■f' 

1 




[p]l = 


■f' 

1 




1 






1 



(c) A wrench given by a moment n and a force f applied at point 
P acts on the EE of the same manipulator at the posture de- 
scribed in item (a) above. Calculate the joint torques or moments 
required to balance this wrench, which is given by 



n]i = 


■f' 

1 


T, 


[f]i = 


■f' 

1 




1 






1 



4.22 Show that the maximum manipulability p, = Y^det(JJ'^) of an orthogonal 
spherical wrist is attained when all three of its axes are mutually orthogo- 
nal. Find that maximum value. 
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4.23 Find an expression for the condition number of a three- re volute spherical 
wrist of twist angles oi and 02 , and show that this number depends only on 
ai, a 2 , and the intermediate joint angle, <? 2 - Moreover, find values of these 
variables that minimize the condition number of the manipulator. Hint: To 
find the required expression, the use of the condition number based on the 
Frobenius norm is strongly recommended. However, rendering the Jacobian 
matrix isotropic can be done by inspection. 

4.24 For the manipulator of Fig. If, with the dimensions of Exercise 4.8, find 
the characteristic length, as defined in Section 4.9. 

4.25 Manipulability of decoupled manipulators. Let pa and represent 
the manipulability of the arm and the wrist of a decoupled manipulator, 

i-e., 

Pa = Y^det(J2i Pw = det( J12 J^2 ) 

with J 12 and J 21 defined in Section 4.5. Show that the manipulability p 
of the overall manipulator is the product of the two manipulabilities given 
above, i.e.. 



4.26 Consider a planar two-revolute manipulator with link lengths ai and a^. 
Find an expression of the form «(r, <? 2 ) for the condition number of its 
Jacobian, with r = 02 / 01 , and establish values of r and O 2 that minimize 
K, which reaches a minimum value of unity. 

4.27 Shown in Fig. 4.29 is an orthogonal three-revolute manipulator with an 
isotropic Jacobian. Find the volume of its workspace. Now consider a second 
manipulator with a similar orthogonal architecture, but with more common 
dimensions, i.e., with links of equal length A. If the two manipulators have 
the same reach, that is, if 



find the volume of the workspace of the second manipulator. Finally, deter- 
mine the KCI — see Section 4.9 for a definition of this term — of the second 
manipulator. Draw some conclusions with regard to the performance of the 
two manipulators. 




5 Trajectory Planning: Pick-and-Place Operations 

5.1 A common joint-rate program for pick-and-place operations is the 
trapezoidal profile of Fig. 13, whereby we plot s'ir) vs. t, using the 
notation introduced in Chapter 5, i.e., with sir) and t defined as 
dimensionless variables. Here, s'ir) starts and ends at 0. From its 
start to a value s'ir) grows linearly, until reaching a maximum 
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then, this function remains constant until a value T 2 is reached, 
after which the function decreases linearly to zero at the end of the 
interval. 

Clearly, this profile has a discontinuous acceleration and hence, is 
bound to produce shock and vibration. However, the profile can be 
smoothed with a spline interpolation as indicated below. 

(a) Find the value of iii terms of ti and T2 so that s(0) = 0 
and s(l) = 1. 

(b) Plot s(t) with the value of found above and decompose it 
into a linear part s;(t) and a periodic part Sp(r). 

(c) Sample s(t) with N equally spaced points and find the periodic 
spline that interpolates Sp(r), for ti = 0.2 and T 2 = 0.9. Try 
various values of N and choose the one that (a) is the smallest 
possible, (b) gives a “good” approximation of the original s(t), 
and (c) yields the best-behaved acceleration program, i.e., an ac- 
celeration profile that is smooth and within reasonable bounds. 
Discuss how you would go about defining a reasonable bound. 

5.2 An alternative approach to the solution of the foregoing smoothing 
problem consists of solving an inverse interpolation problem: Plot 
the acceleration program of the foregoing joint-rate plot, Now, 

sample a set of N equally spaced points of s"(t) and store them in 
an A^-dimensional array s". Next, find the ordinates of the support- 
ing points of the interpolating periodic spline and store them in an 
array s of suitable dimension. Note that s" does not contain infor- 
mation on the linear part of sir). You will have to modify suitably 
your array s so that it will produce the correct abscissa values of the 
interpolated curve s(t), with s(0) = 0 and s(l) = 1. Moreover, sir) 
must be monotonic. Try various values of N and choose the smallest 
one that gives a well-behaved acceleration program, as described in 
Exercise 5.1. 




FIGURE 13. A trapezoidal joint-rate profile. 
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5.3 One more approach to smoothing the joint-rate profile of Fig. 13 is 
to use cycloidal motions. To this end, define a segment of a cycloidal- 
motion function between t = 0 and t = ti, so that s'(ti) = 

for sJjjax indicated in the same figure. Further, define a similar 
segment between t = T2 and t = 1 so that s'(t 2 ) = s^ax = 

0. Then, join the two segments with a line of slope s(j,ax- Plot the 
displacement, velocity, and acceleration of the smoothed motion. Note 
that the smoothed profile must meet the end conditions s(0) = 0 and 
s(l) = 1, and that you may have to introduce a change of variable to 
shrink the corresponding s'(t) segment to meet these conditions. 

5.4 A pick-and-place operation involves picking objects from a magazine 
supplied with an indexing mechanism that presents the objects with 
a known pose and zero twist, at equal time-intervals T, to a robot, 
which is to place the objects on a belt conveyor running at a constant 
speed vq. Find 5th- and 7th-degree polynomials that can be suitably 
used to produce the necessary joint- variable time- histories. 

5.5 Repeat Exercise 5.4, but now the objects are to be picked up by the 
robot from a belt conveyor traveling at a constant velocity vi and 
placed on a second belt conveyor traveling at a constant velocity V 2 . 
Moreover, let pi and p 2 designate the position vectors of the points 
at the pick- and the place poses, respectively. Furthermore, the belts 
lie in horizontal, parallel planes. Finally, the objects must observe the 
same attitude with respect to the belt orientation in both the pick- 
and the place poses. 

5.6 Approximate the cycloidal function of Subsection 5.4 using a peri- 
odic cubic spline with N subintervals of the same lengths, for various 
values of N between 5 and 100. Tabulate the approximation error e^r 
vs. N, with defined as 

cn = max{ej}(^ 
i 

and 

6j = max |s(t) - c(t)|, < t < Tj+i 

T 

in which sir) denotes the spline approximation and c(t) the cycloidal 
function. Note: the cycloidal function can he decomposed into a linear 
and a periodic part. 

5.7 From inspection of the plot of the 3-4-5 polynomial and its deriva- 
tives displayed in Fig. 5.2, it is apparent that the polynomial can be 
regarded as the superposition of a linear and a periodic function in 
the interval 0 < t < 1. Approximate the underlying periodic func- 
tion with a periodic cubic spline by subdividing the above-mentioned 
interval into N equal subintervals, while finding the value of N that 
will yield a maximum absolute value of less than 10^*^ in the error in 
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(a) the function values; 

(b) the values of the first derivative; and 

(c) the values of the second derivative. 

5.8 Repeat Exercise 5.7 for the 4-5-6-7 polynomial of Fig. 5.3. 

5.9 A pick-and-place operation is being planned that should observe man- 
ufacturer’s bounds on the maximum joint rates delivered by the mo- 
tors of a given robot. To this end, we have the following choices: (a) 
a 4-5-6-7 polynomial; (b) a symmetric trapezoidal speed profile like 
that of Fig. 13, with ti = 0.20; and (c) a cycloidal motion. Which 
of these motions produces the minimum time in which the operation 
can be performed? 

5.10 The maximum speed of a cycloidal motion was found to be 2. By 
noticing that the cycloidal motion is the superposition of a linear 
and a periodic function, find a cubic-spline motion that will yield 
a maximum speed of 1.5, with the characteristics of the cycloidal 
motion at its end points. 

5.11 The acceleration of a certain motion s(t), for 0 < t < 1, is given at 
a sample of instants { tj, in the form 

s"(Tfc) = Asin(27TTfc) 

Find the cubic spline interpolating the given motion so that its sec- 
ond time-derivative will attain those given values, while finding A 
such that s(0) = 0 and s(l) = 1. Hint: A combination of a linear 
function and a periodic spline can yield this motion. In order to find 
the function values of the periodic spline, exploit the linear relation 
between the function values and its second derivatives at the spline 
supporting points, as discussed in Section 5.6. 

5.12 A robotic joint has been found to require to move, within a time- 
interval T, with a set of speed values { 6k at equally spaced in- 
stants. Find the natural cubic spline that interpolates the underlying 
motion so that the angular displacement undergone from beginning to 
end is a given Ad. Hint: You will need to establish the linear relation 
between the spline function values and those of its first derivative. 
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6.1 Show that: 

(*) the 6n-dimensional manipulator twist lies in the nullspace of the 
6n X 6n manipulator angular velocity matrix W ; 
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(m) the time-derivative of the 6 n x 6 n manipulator mass matrix M 
is given by 

M = WM - MW 

(Hi) 



dfi 

dt 



Mt -h WMt 



thereby verifying eq.(6.15). 



6.2 In order to gain insight into the meaning of vector 7 , as defined in 
Example 6.3.1, we define a similar vector 77 as 



V 



djW) . 

de 



Compute 77 for that example and compare the result with 7 . 



6.3 The decoupled robot of Fig. 9 is to undergo a maneuver, at the pos- 
ture displayed in that figure, that involves the velocity and accelera- 
tion specifications given below, in base coordinates: 



c 



c 



1 

0 

1 

0 

1 

0 



m/s, u) - 
m/s^, u) 



0 

1 

0 



rad/s. 



1 

0 

1 



rad/s^ 



Compute the joint torques required to drive the robot through the de- 
sired maneuver, if the robot is known to have the inertial parameters 
given below: 



mi = 10.521, m 2 = 15.781, m 3 = 8.767, 
m 4 = 1.052, ms = 1.052, ms = 0.351 



Pi = 


0 

-0.054 


, P2 = 


'0.140' 

0 


. P3 = 


0 

-0.197 




0 




0 




0 





0 




0 




0 


Pi = 


0 

-0.057 


j Ph — 


-0.007 

0 


j Pe ^ 


0 

-0.019 



diag[ 1.6120 


0.5091 


1.6120] 


diag [0.4898 


8.0783 


8.2672] 


diag [3.3768 


0.3009 


3.3768] 


diag [0.1810 


0.1810 


0.1273] 


diag [0.0735 


0.0735 


0.1273] 


diag [0.0071 


0.0071 


0.0141] 
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where m*, p^, and Ij are given in units of kg, m and kg m^, respec- 
tively, with the position vectors of the mass centers and the moment- 
of-inertia matrices given in link- fixed coordinates. Note: Assume that 
Zr is perpendicular to and Zq, with O 7 located at the OP of the 
EE. 

6.4 Derive homogeneous, linear constraint equations on the twists of the 
pairs of coupled bodies appearing in Fig. 14, namely, 

(a) two rigid pulleys coupled by an inextensible belt, under no slip; 

(b) the bevel pinion-and-gear train with axes intersecting at an ar- 
bitrary angle a; 

(c) the cam-and-follower mechanism whose cam disk is an eccentric 
circular disk. 

Notice that the constraint equations sought should have the- 
form: 

Ati + Bt2 = 0 

with ti and t 2 denoting the twists of bodies 1 and 2 , respectively. 

6.5 Use the expressions derived in Example 6.6.1 with the aid of the 
natural orthogonal complement, as pertaining to the planar manip- 
ulator of Fig. 6.1, to obtain an expression for the time-derivative of 
the inertia matrix of this manipulator. Compare the expression thus 
obtained with that derived in Example 6.3.1, and verify that the dif- 
ference I — 2C is skew-symmetric — see Exercise 10.2 — where C is the 
matrix coefficient of the Coriolis and centrifugal terms. 

6.6 A three-revolute spherical wrist with an orthogonal architecture, i.e., 
with neighboring joint axes at right angles, is shown in Fig. 15. As- 
sume that the moments of inertia of its three links with respect to 
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O, the point of concurrency of the three axes, are given by constant 
diagonal matrices, in link-fixed coordinates, as 

14 = diag(Ji, J2, J3) 

1 5 =diag(ifi,if2,if3) 
le = diag(Li,L2,i3) 

while the potential energy of the wrist is 

V = —rriQgacosO^ 

Moreover, the motors produce torques T4 ,ts, and respectively, 
whereas the power losses can be accounted for via a dissipation func- 
tion of the form 

A = ^ +Tf\9i\^ 

where 6j and rf , for * = 4 , 5 , 6, are constants. 

(a) Derive an expression for the matrix of generalized inertia of the 
wrist. 

(b) Derive an expression for the term of Coriolis and centrifugal 
forces. 

(c) Derive the dynamical model of the wrist. Hint: The kinetic en- 

ergy T of a rigid body rotating about a fixed point O with angu- 
lar velocity lo can be written as T = where Iq is the 

moment- of-inertia matrix of the body with respect to O . 




FIGURE 15. A three-revolute spherical wrist. 
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6.7 Shown in Fig. 16 is a two-revolute pointing manipulator. The cen- 
troidal inertia matrices of the links are denoted by Ii and I 2 . These 
are given, in link-fixed coordinates, by: 





'hi 


I12 


Il 3 




Jll 


J12 


Jl 3 


Il = 


I12 


I 22 


I23 


, l 2 = 


J12 


J22 


J23 




_Il 3 


I23 


I33 _ 






J23 


J33 _ 



Moreover, the mass centers of the links are denoted by Ci and C* 2 , 
respectively, and are shown in the same figure, the masses being de- 
noted by mi and m 2 . 

(a) Determine the kinetic energy of the manipulator as a quadratic 
function of 9i and <? 2 - 

(b) Determine the 2x2 matrix of generalized inertia. 

(c) Find an expression for the time-rate of change of the matrix 
of generalized inertia by straightforward differentiation of the 
expression found in item (b). 

(d) Repeat item (c), but now by differentiation of the three factors 
of I, as given in 

I = T^MT 

6.8 The twist tj of the *th link of an n-dof serial manipulator can be 
expressed as 

tj = Tj0 

where Tj is a 6 x n link-twist-shaping matrix and 9 is the n-di- 
mensional vector of actuated joint rates. Moreover, let Mj and Wj 
be the 6x6 matrices defined in Section 6.3. Show that if the link is 
constrained to undergo planar motion, then the product T^WjMiTj 
vanishes. 




FIGURE 16. A two-revolute pointing manipulator. 
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6.9 Devise a recursive algorithm to compute the joint torques required 
to balance a wrench w acting at the EE of a six-revolute manipu- 
lator of arbitrary architecture. Then, derive the number of floating- 
point operations (multiplications and additions) required to compute 
these torques, and compare your result with the number of float- 
ing point operations required to compute the same by matrix-times- 
vector multiplications, using the transpose Jacobian. 

6.10 Establish the computational cost incurred in computing the term of 
Coriolis and centrifugal forces of an n-revolute serial manipulator, 
when the Newton-Euler algorithm is used for this purpose. 

6.11 Shown in Fig. 17 is an RRP manipulator, whose DH parameters are 
displayed in Table 2. The masses of its three moving links are denoted 
by mi, m 2 , and m 3 , and the mass center of each of links 1 and 2 
coincides with Oi, while the mass center of link 3 is located at P. 
Moreover, the centroidal moments of inertia of these links are, in 
link- fixed coordinates, 

[Ii] 2=M1, [I2]3 = S1, [l3]4 = C’l 

where 1 denotes the 3x3 identity matrix. 

(a) Derive the Euler-Lagrange equations of the manipulator under 
the assumption that gravity acts in the direction of Xi. 

(b) Find the generalized inertia matrix of the manipulator. 

6.12 A link is said to be inertially isotropic if its three principal moments 
of inertia are identical. 




FIGURE 17. An RRP spatial manipulator. 




7 Special Topics on Rigid-Body Kinematics 487 



TABLE 2. DH parameters of the RRP manipulator 



i 


OjI 


hi 




1 


0 


0 


0 

0 


2 


0 


0 


0 

0 


CO 


0 


bs 


0 

0 



(a) Show that any direction is a principal axis of inertia of an iner- 
tially isotropic link. 

(b) Explore the advantages of a manipulator with inertially isotropic 
links with regard to its real-time control, i.e., find the savings 
in floating-point operations required to compute the recursive 
Newton-Euler algorithm of such a manipulator. 

6.13 Devise an algorithm similar to Algorithm 6.6.1, but applicable to 
planar manipulators, and determine the computational costs involved 
in its implementation. 

6.14 Write a piece of code to evaluate numerically the inertia matrix of an 
n-axis manipulator and test it with the manipulator of Example 6.6.1. 
For this purpose, assume that I = ma? . 



7 Special Topics on Rigid-Body Kinematics 

7.1 The regular tetrahedron of Fig. 7, of unit-length edges, moves in such 
a way that vertex Pi has a velocity of unit magnitude directed from 
Pi to P 4 , whereas the velocity of P 2 is parallel to edge P 2 Ps- Define a 
coordinate frame X, Y, Z with origin at Pi, Y axis directed from Pi 
to the midpoint M of P2P3, and X axis in the plane of Pi, P 2 , P 3 , 
as shown in that figure. With the above information, 

(a) And the velocity of P 2 ; 

(b) show that the velocity of P 3 cannot be zero; 

(c) if the velocity of P3 lies in the P1P2P3 plane. And that velocity; 

(d) And the angular velocity of the tetrahedron; 

(e) And the set of points of the tetrahedron undergoing a velocity 
of minimum magnitude. 

7.2 The position vectors of three points of a rigid body, pi, p2, and p3, 
as well as their velocities, pi, p2, and p3, are given below: 





T ' 




1 ■ 




■- 1 ' 


Pi = 


1 


, P2 = 


-1 


, P3 = 


1 




1 




1 




-1 
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T 




■ 3 ■ 




■-T 


Pi = 


1 


, P2 = 


1 


, P3 = 


1 




1 




-1 




3 



(a) Is the motion possible? 

(b) If the motion is possible, find its angular velocity. 

7.3 For matrix P defined as in eq.(7.4), i.e., as 

P = [pi - C P2 - C P3 - C] 

where { p^ are the position vectors of three points of a rigid body, 
while c is that of their centroid, prove that tr(P^) = tr^(P) whenever 
the origin and the three given points are collinear. 

7.4 With matrix P defined as in Exercise 7.3 above, prove Theorem 7.2.3. 
That is, prove that 

tr(PP^) = 0 

7.5 With the notation of Section 7.3, prove that 

vect(ri^P) = Dtu 

7.6 Derive the velocity and acceleration compatibility conditions for a 
body that is known to undergo spherical motion, i.e., a motion under 
which one point of the body remains fixed. 

7.7 The position vectors of three points of a rigid body, pi, p2, and pa, 
are given as in Exercise 7.2, and repeated below for quick reference: 





T 




1 ■ 




■- 1 ' 


Pi = 


1 


, P2 = 


-1 


, P3 = 


1 




1 




1 




-1 



However, the velocities of these points are all zero, while their accel- 
erations are given as 





T 




■ 3 ■ 




■-1' 


Pi = 


1 


, P2 = 


1 


, P3 = 


1 




1 




-1 




3 



(a) Show that the motion is compatible. 

(b) Find the angular acceleration of the body. 

7.8 With reference to Example 7.2.1, compute the angular acceleration 
of the cube of Fig. 7.1c if p* = 0, for *=1,2, 3. 
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7.9 With the notation of Section 7.2, let 



R = PP^ 

(a) Show that the moment of inertia J of the three given points, 
which is identical to that of a system of unit masses located at 
these points, with respect to the centroid C of the given points, 
is 

J = tr(R)l — R 

(b) Show that if the three given points move as points of a rigid body 
undergoing an angular velocity u> whose cross-product matrix is 

then 

j = Rf2 - OR 

(c) Furthermore, show that if under the conditions of item (b) above, 
the set of points undergoes an angular acceleration u> of cross- 
product matrix f2, then 

J = Rii - OR - n^R - RO^ + 2f2Rr2 

7.10 A wrench of unknown force f is applied to a rigid body. In order to 
find this force, its moment with respect to a set of points {Pk}i, 
of position vectors { Pfc }i, is measured and stored in the set { rifc }®. 
Show that f can be calculated from the relation 

Df = — vect(M) 

with D defined as in Section 7.2, i.e., as 

D^l[tr(P)l-P] 



and M given by 

M = [ 111 — n n2 — n na — n 




Note that P is defined in Exercise 7.3. 

7.11 A wrench is applied to the tetrahedron of Fig. 7. When the force of 
this wrench acts at point Pk, the resulting moment is rifc, for k = 
1, 2, 3. For the data displayed below, in frame P of that figure, find 
the resultant force f, as well as the line of action of this force that will 
lead to a moment of minimum magnitude. Determine this moment. 



V2 

111 = — — 


T 


1 

’ “2 = 7X 


■ 3a/2 ■ 


1 

’ “3 = TX 


■ 3a/2 ■ 


0 


-2a/6 


2a/6 


4 


0 


12 


2a^ 


12 


-2a/3 
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7.12 Matrix D, as defined from eq.(7.6) and displayed in eq.(7.10), was 
found to involve frequent singularities, even in the presence of non- 
collinear points. This weakness stems from its lack of frame-invariance, 
and can be readily fixed if both sides of eq.(7.6) are multiplied by 
from the right. Show that, under these conditions, an equation simi- 
lar to (7.9) is derived, but with D replaced by (1/2) J, with J defined 
as in Exercise 7.9. Now show that J is frame-invariant in the sense of 
Section 2.7, and becomes singular if and only if the three given points 
are collinear. 



8 Kinematics of Complex Robotic Mechanical 
Systems 

8.1 Show that the left-hand side of eq.(8.22f) represents a pure reflection 
of vector h about a plane of unit normal f/||f||, if multiplied by ||f|8. 
Also show that the right-hand side of the same equation represents 
a pure reflection of vector i about a plane of unit normal g/||g||, if 
multiplied by ||g| 8 . 

8.2 Show that and y 2 , as defined in eqs. (8.46c & d) both vanish. 

8.3 In this exercise, we will try to gain insight into the consequence of 
the double point at 0 /^ = 9 ^= tt/ 2 of Fig. 8.4 of Example 8.2.2. 
To this end, show that, for this combination of values, matrix H of 
eq. ( 8 . 68 a) becomes zero, and hence, X 3 cannot be computed from this 
equation. As a result, none of the remaining angles can be computed 
recursively. 

8.4 As an alternative approach to the 14 fundamental equations derived 
in Section 8.2, we recall eqs. (8. 13a & b), if written in a more conve- 
nient form, so as to have a minimum number of matrix multiplica- 
tions, namely. 



Q3Q4Q5 = QfQfQQf 

Qf (ai - p) + Q^a2 + as + Qaa4 

+ Q3Q4a5 + Q3Q4Q5ae = 0 

Now equate the four linear invariants of the two sides of the first of the 
two foregoing equations. The result is a set of four scalar equations. 
When the translational equations are expanded, and appended to the 
first four equations, a system of seven trigonometric equations in the 
six unknown angles is derived. Obtain that system of seven equations 
and comment on their suitability to solve the IKP. 
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8.5 In Subsection 8.2.5 we realized that, upon applying the Raghavan 
and Roth elimination method, and once 9^ is computed, 64 and 9^ 
can be computed at once by finding the eigenvector of S associated 
with its zero eigenvalue. While this calculation can be performed 
with the eigenvalue-computation module of any scientific package, 
computing the eigenvalues of a 12 x 12 matrix like S requires an 
iterative procedure, which can be time-consuming, especially if this 
computation is only a part of a more complex procedure. 

In order to find X45, and hence, 94 and 9 ^, from eq.(8.51d), we need 
not resort to a full eigenvalue problem. Instead, a vector v can be 
computed directly^ as opposed to iteratively, that spans the nullspace 
of S, for a given computed value of 9^„, if a change of variables is 
introduced that will yield S in upper-triangular form. In fact, since S 
is a fortiori singular, its last row is bound to have zero entries in that 
form. Devise an algorithm that will render S in upper-triangular form 
and hence, compute vector X45 under the conditions that this vector 
(a) lie in the nullspace of S and (b) its 12th entry be unity. Hint: 
Apply an orthogonalization procedure, as described in Appendix B. 

8.6 For the parallel manipulator of Fig. 8.7, find the matrix mapping 
joint forces into wrenches acting on the moving platform, if actuation 
is supplied through the prismatic joints. 

8.7 Show that determinants Ai, A 2 , and A 3 of Example 8.3.2 are 14th-, 
13th-, and 12th-degree polynomials in T 2 , respectively. 

8.8 What is the counterpart of a decoupled serial manipulator, as de- 
scribed in Section 4.4, of a six-dof parallel manipulator with the ar- 
chitecture of Fig. 8.7? What is the degree of the characteristic poly- 
nomial of that parallel manipulator? Compare this answer with the 
characteristic polynomial derived in Section 4.4. 

8.9 We refer to the rolling robot with conventional wheels introduced 
in Subsection 8.6.1. We would like to study the equivalent concept 
of manipulability, which here we can call maneuverability. This con- 
cept refers to the numerical conditioning of the two underlying Ja- 
cobian matrices, J and K, as defined in eqs.(8.117a & b). Clearly, J 
is isotropic and hence, optimally conditioned. In attempting to de- 
termine the condition number of K, however, we need to order its 
singular values from smallest to largest. 

(a) Show that the two singular values of K are <ti = l/r and <T2 = 
2/r. Obviously, an ordering from smallest to largest is impossible 
because of the lack of dimensional homogeneity. 

(b) In order to cope with the dimensional inhomogeneity of matrix 
K, we introduce the characteristic length L, which we define 
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below. First, we redefine the Jacobian K in dimensionless form 
as 



K 



(l/r) 0 0 

0 0 2L/r 



Now, L is the value that minimizes the condition number of the 
dimensionless K. Show that this value is 1/2 and that it produces 
a condition number of unity. 



8.10 Find an expression for the angular velocity 4>i of the active roller 
of the *th wheel of the robot with Mekanum wheels introduced in 
Subsection 8.6.2. 



8.11 We refer again to the robot with Mekanum wheels introduced in Sub- 
section 8.6.2. For the case of a three-wheeled robot of this kind, we 
consider here a design whereby the wheels are equally spaced in a A- 
array. In this array, the centers of the hubs, Oj, he at the corners of 
an equilateral triangle of side a; moreover, we assume that Oj = 90°, 
for * = 1, 2, 3. Under these conditions, find the characteristic length 
L of the robot that renders K, as defined in the above-mentioned 
subsection, dimensionless and of a minimum condition number. Find 
this minimum as well. 



8.12 Find the value of t/ at which the rolling robot of Fig. 8.22 attains 
a singular configuration. Here, a singularity is understood as a loss 
of maneuverability in the sense of not being able to drive the unac- 
tuated joints by means of the actuated ones. Discuss whether under 
reasonable values of the geometric parameters, this singularity can 
occur. 



8.13 Determine the architecture and the “posture”, i.e., the values of the 
relevant joint variables of the rolling robot of Fig. 8.22 that will render 
matrix 0 isotropic, where 0 represents the mapping of actuated 
joint rates into unactuated ones. Is kinematic isotropy, in this sense, 
kinematically possible? 

8.14 Find a relation among the geometric parameters of the robot of 
Fig. 8.22 that will allow the steering of the robot along a straight 
course with the highest possible maneuverability in the sense defined 
in Exercise 8.12. That is, find a relation among the geometric param- 
eters of this robot that will render k(0) a minimum along a straight 
course. 



8.15 Find the value of 'tp under which the robot of Fig. 8.22 performs a ma- 
neuver that leaves the midpoint of segment O 1 O 2 stationary. Under 
this maneuver, state a relationship among the geometric parameters 
of the robot that minimizes k ( 0 ). 
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8.16 Upon inversion, eq.(8.128a) yields 

0 a = V0 



(a) Find U. 

(b) The above equation can be written as 

01 = M13^3 + Miy V’ = uf 0u 

02 = U2303 + W2y V’ = 9u 

The first of the above equations can be integrated if ui, which 
is an implicit function of 9^ and 'ip, is the gradient with respect 
to 9u = [03 of a scalar function Ui{9s,ip). Likewise, the 

second of the above equations can be integrated if a function 
U 2 { 03 ,'>P) exists, whose gradient with respect to is U 2 . Fur- 
ther, upon recalling Schwartz’s Theorem of multivariable calcu- 
lus, Uj is such a gradient if and only if Vuj, i.e., the Hessian 
matrix of Ui with respect to 0„, is symmetric, for * = 1,2. 

Show that the above-mentioned Hessians, for the case at hand, 
are nonsymmetric, and hence, none of the above differential 
expressions is integrable. Such expressions are called nonholo- 
nomic. 

Note; To be sure, the above condition is sufficient, but not nec- 
essary. It is possible that some individual equations of a system 
of differential expressions, also called Pfaffian forms, are not 
integrable while the overall system is. An examination of neces- 
sary and sufficient conditions for integrability falls beyond the 
scope of this book. Such conditions are best understood with the 
aid of the Frobenius Theorem (De Luca and Oriolo, 1995) and 
its analog, the Holonomy Theorem (Ostrovskaya and Angeles, 
1998). 

8.17 For the rolling robot with omnidirectional wheels introduced in Sec- 
tion 8.6.2, with a A-array, as described in Exercise 8.11, show that 
the equation yielding the angular velocity of the platform in terms of 
the wheel rates is integrable, but the equations yielding the velocity 
of the operation point are not. 

8.18 A holonomic rolling robot. The robot described in Exercise 8.17 
can be rendered holonomic at the expense of one degree of freedom. 
Show that if the three wheel rates are coordinated, either mechani- 
cally or electronically so that 

+ 02 + 03 = 0 
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then the platform is constrained to move under pure translation. 
When operating in this mode, the robot is holonomic. Find an ex- 
plicit expression for the position vector c of the operation point in 
terms of the wheel angles. 



9 Trajectory Planning: Continuous-Path 
Operations 

9.1 A PUMA 560 robot, with the DH parameters of Table 3, is used to 
perform a gluing operation as indicated below: A nozzle dispensing 
the glue is rigidly attached to the gripper of the robot. The tip of 
the nozzle, point P, is to trace a helicoidal path at a constant rate 
of 50 mm/s. Furthermore, the center of the wrist is located at a 
point C, fixed to a Ffenet-Serret coordinate frame. In this frame, the 
coordinates of C are (0, —50, 86.7) mm. Moreover, the path to be 
traced by point P is given as 

X = a cos'd, y = usin'!?, z = 6d, 0 < d < tt/2 
with the values a = 300 mm, b = 800/tt mm. 

(a) Decide where to locate the robot base with respect to the path 
so that the latter will he well within the workspace of the robot. 
Then, produce plots of 9i vs. t, for 0 < t < T, where T is the 
time it takes to traverse the whole trajectory, for * = 1, 2, . . . , 6. 

(b) Produce plots of 9i vs. t in the same time interval for all six 
joints. 

(c) Produce plots of 9i vs. t in the same time interval for all six 
joints. 

9.2 A bracket for spot-welding, shown in Fig. 18, is rigidly attached to the 
end-effector of a robotic manipulator. It is desired that point P of the 
bracket follow a helicoidal path P, while keeping the orientation of 
the bracket with respect to P as indicated below: Let B = {io, Jo,ko} 

TABLE 3. DH parameters of a PUMA 560 robot 



Joint i 


tti (deg) 


tti (m) 


bi (m) 


1 


90 


0 


0.660 


2 


0 


0.432 


0 


3 


90 


0.020 


0.149 


4 


90 


0 


0.432 


5 


90 


0 


0 


6 


0 


0 


0.056 
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and T’j = {irJrjkr} be triads of unit orthogonal vectors fixed to 
the base of the robot and to the bracket, respectively. Moreover, let 
T = {ej, e„, 65 } be the Ffenet-Serret triad of T, given as 

6 t = — 0.6 sin y>io + 0.6 cos y>jo + 0 . 8 ko 

e„ = - cos y-io - sin y>jo 

65 = 0.8 sin y>io — 0.8 cos y>jo + 0 . 6 ko 

where is a given function of time, y(t). 

Furthermore, the orientation of the bracket with respect to T is to 
be kept constant and given in terms of the Ffenet-Serret triad as 

ir = 0.933et + 0.067e„ — 0.354e6 
jr = 0.067ej + 0.933e„ + 0.354e6 
kr = 0.354et — 0.354e„ + 0.86665 

Additionally, R and S(t) denote the rotation matrices defining the 
orientation of JT 7 with respect to JT and of JT with respect to B, 
respectively. 

(a) Find the matrix representation of S(t) in B. 

(b) Find the matrix representation of R in T . 

(c) Let Q(t) denote the orientation of JT 7 with respect to B. Find 
its matrix representation in B. 

(d) Find the Darboux vector d of the path, along with its time- 
derivative, 5, in base-fixed coordinates. Note: You can do this 
in several ways, as discussed in Section 9.2. Choose the one 
that will allow you to use previously computed results, thereby 
simplifying the computations. 

9.3 The parametric equations of a curve are given as 

X = 2t, y = f^ , z = t®/3 

where t is time. A robotic manipulator is to follow this trajectory 
so that its gripper keeps a constant orientation with respect to the 
Ffenet-Serret frame of the curve. 

(a) Determine the unit vector parallel to the axis of rotation and 
the angle of rotation of the gripper as functions of time. 

(b) Find the angular velocity and angular acceleration of the gripper 
as functions of time. 



9.4 Derive eqs. (9.45a & b). 
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9.5 Find the spline approximation of the helix of Example 9.3.1. Then, 
plot the approximation errors of the Cartesian coordinates of points 
of the helix, for N = 5, 11, and 21 equally spaced supporting points. 
In order to assess the orientation error, compute the Darboux vectors 
of the spline, Sg, and of the helix, Sh- The approximation error of the 
orientation is now defined as 

Co = max{||5s(F) ~ 
with (f defined as in Example 9.3.1. 

9.6 Find the spline approximation of the curvature, torsion, and Darboux 
vector of the curve introduced in Example 9.3.2. Find expressions for 
the exact values of these variables and plot the approximation errors, 
for 5, 10, and 20 equally spaced supporting points vs. y>. In the error 
definitions given below, subscript e indicates exact value, subscript s 
spline value: 

= Ks{cp) - Ke(p) 

Cr = Ts(p) - Te{cp) 

es = II^s(f) - ^e(F)ll 



9.7 From the plots of the time- histories of the joint angles calculated in 
Example 9.5.1, it is apparent that, with the exception of 64 , which 
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has a linear component, these histories are periodic. Repeat Exam- 
ple 9.5.1, but now using a spline approximation of the welding seam, 
with N = 5, 10, and 20 supporting points. With this spline approxi- 
mation, calculate the pose, the twist, and the twist-rate at each sup- 
porting point. Now, calculate values of 6, 6, and 9 at each of these 
supporting points by means of inverse kinematics. Compare the val- 
ues thus obtained of 9 with those derived from the linear relation 
between the function values and the values of its second derivative 
at the supporting points when using a cubic spline. Tabulate the Eu- 
clidean norm of the errors vs. N. 

9.8 The decoupled robot of Fig. 9 is to perform an arc-welding operation 
along a welding seam that requires its wrist center C to travel at a 
constant speed of 1 m/s along a line joining points A and B, not 
shown in that figure, while keeping the EE holding the electrode at 
a constant orientation with respect to the base frame. Moreover, the 
seam is to be traversed according to the following schedule: With 
point C located at a point A! on the extension of AB, a distance 
of 250 mm from A, point C approaches A with a cycloidal motion 
at the specified speed; upon reaching B, point C decelerates with 
a cycloidal motion as well, until it reaches a point B' in the other 
extension of AB, 250 mm from B, with zero speed. The position 
vectors of points A and B, denoted by a and b, respectively, are 
given, in base coordinates, as 





■ 500 ■ 




■ 1,200' 


a = 


-500 


, b = 


0 




500 




1,200 



in mm. For the above-given data, find the time- histories of all joint 
variables. 

9.9 Derive expressions (9.45a & b). 

9.10 If linear invariants are used to represent the desired pose s^, then 
q = 0 and </o = 1 when the angle of rotation becomes tt. Under these 
conditions, matrix T of eq.(9.72), this equation thus not necessarily 
leading to eq.(9.73). One way of coping with this algorithmic singu- 
larity consists in redefining axis Xi of the DH notation by rotating 
the current Xi axis by an angle A9i about Z\, which does not affect 
the remaining variables and parameters of the said notation. 

Find the optimum value of A9i that will take T “farthest” from its 
current rank-deficiency. 
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10 Dynamics of Complex Robotic Mechanical 
Systems 

10.1 Show that the mathematical model of an arbitrary robotic mechanical 
system, whether holonomic or nonholonomic, with r rigid bodies and 
n degrees of freedom, can be cast in the general form 

I( 0 ) 0 „ + C( 0 , 0 „) 0 „ = M +7 + 5 

where 

6. the m-dimensional vector of variables associated with all joints, 
actuated and unactuated; 

6a'. the n-dimensional vector of actuated joint variables, n < m', 

T^'. the n-dimensional vector of actuator torques; 

7 : the n-dimensional vector of gravity torques; 
d'. the n-dimensional vector of dissipative torques; 

1(0): the n X n matrix of generalized inertia; 

C(0, 9a)'. the n X n matrix of Coriolis and centrifugal forces; 

with 1(0) and C(0, 9a) given by 

1(0) = T^MT 

C(0, 9a) = ^[i + T^Mt - t^MT + T^(WM + MW)T] 

in which 

M: the 6 r x 6 r matrix of system mass; 

T: the n x 6 r twist-shaping matrix that maps the n-dimensional 
vector of actuated joint rates into the 6 r-dimensional vector of 
system twist t; 

W : the 6 r x 6 r matrix of system angular velocity. 

10.2 For the system of Exercise 10.1, show that the matrix difference 
1(0, 0„) -2C(0, 9a) is skew-symmetric. This is a well-known result 
for serial manipulators (Spong and Vidyasagar, 1989). 

10.3 For the rolling robot with conventional wheels of Subsection 10.5.1, 
find the generalized inertia matrix of the robot under the maneuvers 
described below: 

(a) pure translation; 

(b) midpoint of segment O 1 O 2 stationary. 




10 Dynamics of Complex Robotic Mechanical Systems 499 



In each case, give a physical interpretation of the matrix thus ob- 
tained. 

10.4 With reference to the same robot of Exercise 10.3, state the conditions 
on its geometric parameters that yield 1^^, and Ip isotropic, these two 
2x2 matrices having been defined in Subsection 10.5.1. 

10.5 Derive the mathematical model governing the motion of a 2-dof rolling 
robot with conventional wheels, similar to that of Fig. 8.22, but with 
two caster wheels instead. The vertical axes of the caster wheels are 
a distance I apart and a distance a + 6 from the common axis of the 
driving wheels. What is the characteristic length of this robot? 

10.6 Find the conditions under which the three-wheeled robot with om- 
nidirectional wheels analyzed in Subsection 10.5.2 has an isotropic 
inertia matrix. Discuss the advantages of such an inertially isotropic 
robot. 

10.7 With reference to the omnidirectional robot of Subsection 10.5.2, 
show that the mathematical model can be manipulated to yield a 
single first-order ordinary differential equation in ct;, of the form 

Cj + kuj = f{t) 

in which A: is a constant with units of frequency, its inverse being the 
time-constant of the system. Find expressions for k and fit). Then, 
integrate the above equation in closed form, to obtain the time- history 
of uj for a given time- history fit) and given initial condition tt^(O). 

10.8 Establish the conditions on the actuated joint rates under which the 
three-wheeled robot with omnidirectional wheels of Subsection 10.5.2 
undergoes pure translation. Under these conditions, the robot has 
only two degrees of freedom and, hence, a 2 x 2 inertia matrix. Derive 
an expression for its inertia matrix. Hint: The constraint for pure 
translation can he written as 



a^0a = 0 

and hence, if the 3x2 matrix L is an orthogonal complement of a, 
i.e., if af'h = Oj, where O 2 is the 2-dimensional zero vector, then the 
underlying Euler- Lagrange equations of the constrained system can he 
derived hy multiplying the two sides of the mathematical model found 
in Subsection 10.5.2 hy if" : 

L^Wa + = L^r - 

Further, upon writing Oa as a linear transformation of a 2-dimensional 
vector u, namely, as 



Oa = Lu 
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we obtain 

L^ILii + L^CLu = \7t - L^5 

and hence, the generalized inertia matrix under pure translation is 

L^IL. 

10.9 Find the maneuver(s) under which the Coriolis and centrifugal forces 
of the robot analyzed in Subsection 10.5.2 vanish. Note that in gen- 
eral, these forces do not vanish, even though the generalized inertia 
matrix of the robot is constant. 

10.10 Find the eigenvalues and eigenvectors of the matrix of generalized 
inertia of the 3-dof rolling robot with omnidirectional wheels analyzed 
in Subsection 10.5.2. 



10.11 The Euler-Lagrange equations derived for holonomic mechanical sys- 
tems in Section 10.3, termed the Euler-Lagrange equations of the sec- 
ond kind, require that the generalized coordinates describing the sys- 
tem be independent. In nonholonomic mechanical systems, a set of 
kinematic constraints is not integrable, which prevents us from solv- 
ing for dependent from independent generalized coordinates, the ap- 
plication of the Euler-Lagrange equations as described in that section 
thus not being possible. However, dependent generalized coordinates 
can be used if the Euler-Lagrange equations of the first kind are re- 
called. To this end, we let q be the m-dimensional vector of dependent 
generalized coordinates that are subject to p differential constraints 
of the form 

A(q)q = b(q,t) 

where A is a p x n matrix of constraints and b is a p-dimensional vec- 
tor depending on the configuration q and, possibly, on time explicitly. 
When b does not contain t explicitly, the constraints are termed scle- 
ronomic] otherwise, rheonomic. Furthermore, let n = m — p be the 
degree of freedom of the system. The Euler-Lagrange equations of the 
first kind of the system at hand take on the form 



d ( dL 
dt \dq 



dq 



0 + A^A 



where A is a p-dimensional vector of Lagrange multipliers that are 
chosen so as to satisfy the kinematic constraints. Thus, we regard the 
m dependent generalized coordinates grouped in vector q as indepen- 
dent, their constraints giving rise to the constraint forces A^A. 

Use the Euler-Lagrange equations of the first kind to set up the math- 
ematical model of the rolling robot with omnidirectional wheels stud- 
ied in Subsection 10.5.1. 
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acatastatic systems, 399 
acceleration analysis 

of parallel manipulators, 337 
of rigid bodies, 89 
of serial manipulators, 153 
affine transformation, 19 
AI (see artificial intelligence) 
algorithm definition, 452 
angle of rotation, 28 
angular acceleration 
computation, 278 
invariant-rate relations, 91 
matrix, 89 
vector, 89 
angular velocity 
dyad, 211 

invariant-rate relations, 88, 
437-439 
matrix, 81 
vector, 81 
Appendix A, 433 
Appendix B, 441 
arc-welding, 362, 376 
operation, 376 
path tracking, 387 



architecture (manipulator -), 107 
articulated-body method, 242 
artificial intelligence, 4, 452 
ABB-IRB 1000 robot, 468, 472 
axial component of a vector, 21 
axial vector of a 3 x 3 matrix, 32 



base frame, 112 
basis of a vector space, 21 
Bezout’s method, 333 
bivariate-equation approach, 299 



C, 452 
C-| — h, 452 

Canadarm (see Canadarm2) 
Canadarm2, 5 

canonical form of a rotation, 31 
Carausius morosus, 13, 501 
Cartesian coordinates 
of a manipulator. 111 
Cartesian decomposition, 31 
caster wheel, 356 
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catastatic systems, 399, 406 
Cayley-Hamilton theorem, 26 
Cayley’s theorem, 454 
change of basis, 56 
characteristic equation, 23, 26, 

119 

characteristic length, 172 
characteristic polynomial, 22, 

151, 286 

Chasles’ theorem (see Mozzi-Chas- 
les’ theorem) 

Chebyshev norm, 171, 390 
Cholesky-decomposition algorithm, 
243, 262 

closure equations (manipulator -), 

113 

compatibility conditions 
for acceleration, 279 
for velocity, 275 

composite rigid-body method, 242 
composition of reflections and rota- 
tions, 45 

condition number, 171, 310 
configuration of a manipulator, 107 
continuous path, 188, 287 
operations, 361 
tracking, 387 
control vector, 232, 263 
coordinate transformation, 46-52 
CP (see continuous path) 

Coriolis 

acceleration, 93 

Coriolis and centrifugal forces, 236, 
242 

Couette flow, 267 
Coulomb 

dissipation function, 268 
friction, 268 

cross-product matrix, 26 
curvature, 364 

derivative w. r. t. a parameter, 

369 

derivative w. r. t. the arc length, 
363 

parametric representation, 369, 370 



time-derivative, 366 
cycloidal motion, 197 



Darboux vector, 365 
time-derivative, 366 
decoupled manipulators, 106, 115 
delta-array (A-array), 423, 487 
Delta robot, 9 
Denavit-Hartenberg 
frames, 105 
notation, 104 
parameters, 107 
rotation matrix, 107, 130 
vector joining two frame origins, 

108, no 

dexterity, 452 

measures (see kinetostatic per- 
formance indices) 

dextrous hands (see multifingered hands) 
dextrous workspace, 169 
dialytic elimination, 152, 470 
DH (see Denavit-Hartenberg) 

DIESTRO manipulator, 183, 317, 318 
inverse kinematics, 286, 319 
Jacobian, 137, 469 
differentiation with respect to vec- 
tors, 26, 27 

direct kinematics problem 

of parallel manipulators, 326 
displacement equations of a manip- 
ulator, 113 
dissipation 

function, 213, 267 
dynamic systems, 1 
dynamics 

of holonomic systems, 399 

of multibody systems, 211 

of parallel manipulators, 402 

of rigid bodies, 97 

of robotic mechanical systems, 398 

of rolling robots, 413 

of serial manipulators, 212 
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EE (see end-effector) 
end-effector, 104 
Euclidean norm, 28 
Euler 

angles, 44, 455 
equation (for graphs), 403 
equation (in mechanics), 100 
parameters (see Euler-Rodrigues 
parameters) 

Euler-Lagrange equations, 216 
derived with the NOG, 241, 402 
Euler-Rodrigues parameters, 41 
Euler’s 

theorem, 25 

formula for graphs (see Euler 
equation for graphs) 



Fanuc Arc Mate robot 

characteristic length, 183 
DH parameters, 184 
inverse kinematics, 316 
KCI, 183 

First Law of Thermodynamics, 158 
flight simulator, 8, 322 
floating-point operation, 156, 242, 
443 

flop (see floating-point operation) 
forward dynamics 

of serial manipulators, 242 
algorithm (using the NOG), 260 
algorithm complexity, 258 
Frenet (see Ffenet-Serret) 
Ffenet-Serret 
frame, 362 
formulas, 364 
vectors, 363 
Frobenius norm, 173 
friction forces, 243, 266 
fuzzy logic, 452 



genealogy of robotic mechanical 
systems, 1, 4 



generalized coordinates, 212, 398 
generalized forces, 212, 421, 422 
generalized inertia matrix, 215 
Gholesky decomposition, 243 
factoring, 244 

time-rate of change, 218, 253 
generalized speeds, 213, 398 
gluing operation, 371 
grasping matrix, 343 
gravity 

terms, 241, 266 
wrench, 233 



hand-eye calibration, 66 
Hexa robot, 9 

holonomic systems, 398, 399 
homogeneous coordinates, 52 
homotopy, 287 



IKP (see inverse kinematics problem) 

Index, 511 

input, 1, 238 

inertia dyad, 100, 211 

instant screw axis, 83 

instrument calibration, 64 

intelligent machines, 2, 452 

intelligent robot, 2 

invariance, 60 

inverse dynamics 

of serial manipulators, 209 
recursive, 210, 221 
inverse kinematics problem 

of a decoupled manipulator, 115 
of a general 6R manipulator, 286 
of parallel manipulators, 324 
inverse vs. forward dynamics, 209 
inward recursions, 228, 230 
ISA (see instant-screw axis) 
isomorphism, 23 
isotropic 

manipulator, 171 
matrix, 170 
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isotropy, 175 
iteration, 390, 452 



Jacobian matrix 

condition number, 169 
evaluation, 142 
invertibility, 169 
of decoupled manipulators, 140 
of serial manipulators, 137 
transfer formula, 140 
joint 

coordinates, 103, 107 
parameters, 106 
variables, 107 



Kane’s method, 221 
KCI (see kinematic conditioning in- 
dex) 

kernel of a linear transformation, 19 
Kinemate, 87 
kinematic 
chain, 104 

conditioning index, 172 
constraints, 234 
constraints for serial manipula- 
tors, 239 
pair, 104 

kinetostatic performance indices, 
168 



Lee’s manipulator, 319 
Lee’s procedure, 307 
Lee vs. Li, 287 
left hand, 9 

Li vs. Lee (see Lee vs. Li) 

Li’s manipulator (see Lee’s mani- 
pulator) 

linear invariants, 31 
of a rotation, 33 
linear transformations, 18 
local structure of a manipulator. 



106 

lower kinematic pair, 104 
LU decomposition, 138 



machine (definition of), 451 
main gauche (see left hand) 
maneuverability, 487 
manipul ability, 168 

of decoupled manipulators, 474 
manipulator 

angular velocity matrix, 211 
architecture, 107 
configuration, 107 
dynamics, 209, 402 
kinematics. 111 
mass matrix, 214 
posture, 107 
twist, 214 
wrenches, 214 
matrix representation, 22 
norm, 173 

mechanical system, 1 
mechatronics, 452 
minimum-time trajectory, 236 
moment invariants, 61 
moment of a line 
about a point, 75 
about another line, 462 
moment of inertia, 97 
momentum screw, 101 
motor, 87 

Mozzi-Chasles’ theorem, 71 
MSS, 6 

multibody system 
dynamics, 211 

Euler-Lagrange equations, 212 
multicubic expression, 114 
multifingered hands, 9, 341 
multilinear expression, 114 
multiquadratic expression, 114 
multiquartic expression, 114 




Index 



519 



natural orthogonal complement, 232 
applied to holonomic systems, 401 
applied to parallel manipulators, 
408 

applied to planar manipulators, 
247 

applied to rolling robots, 422, 431 
Newton 

equations, 100 
-Euler algorithm, 230 
methods, 287, 387 
-Raphson method, 67, 387 
NOG (see natural orthogonal com- 
plement) 

nonholonomic systems, 210, 397 
noninertial base link, 241 
norm (matrix -), 173 

Also see Frobenius norm 
normal component of a vector, 23 
nullspace of a linear transformation, 
21 

numerical conditioning, 299, 310 



object-oriented programming, 452 
Odetics series of hexapods, 13 
ODW (see omnidirectional wheels) 
off-line, 3, 119, 143 
omnidirectional wheels, 16 
dynamics, 423 
kinematics, 355 
on-line, 452 
operation point, 106 
orientation problem, 130 
orthogonal complement, 235 
orthogonal decomposition of a vec- 
tor, 21 

orthogonal decoupled manipulator, 
125 

orthogonal projection, 19 
orthogonal RRR manipulator 
dynamics, 247 
inverse kinematics, 126, 129 
recursive dynamics, 250 



workspace, 150 
OSU ASV, 13 
OSU hexapod, 13 
outward recursions, 222 



Pappus-Guldinus theorems, 472 
parallel axes, theorem of, 99 
parallel manipulators, 8 
acceleration analysis, 337 
dynamics, 402 
kinematics, 319 
velocity analysis, 334 
parametric 

path representation, 368 
representation of curvature, 369 
representation of curvature deri- 
vative, 369 

representation of torsion, 369 
representation of torsion deriva- 
tive, 369 
splines, 381 

path-tracking for arc- welding, 377 
pick-and-place operations, 188 
planar manipulators, 160 
acceleration analysis, 165 
displacement analysis, 160 
dynamics, 252 
static analysis, 158 
velocity analysis, 163 
platform manipulators, 323, 402 
Pliicker coordinates (of a line), 74 
transfer formula, 77 
polar-decomposition theorem, 169 
polynomial interpolation 

with 3-4-5 polynomial, 191 
with 4-5-6-7 polynomial, 194 
pose (of a rigid body), 77 
array, 78 

positioning problem, 115 
posture of a manipulator, 107 
PPO (see pick-and-place operations) 
Principle of Virtual Work, 158 
prismatic pair, 104, 105, 138 
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programmable robot, 4 
projection, 19 
Puma robot, 107, 124 
DH parameters, 107 
inverse kinematics, 115 
workspace, 150 
pure reflection, 20 



quaternions, 43 



Ragliavan and Roth’s procedure, 
301 

range of a linear transformation, 

19, 453 

Rayleigh dissipation function (see 
dissipation function) 
real time, 209, 452 
reciprocal bases, 66, 142, 412 
redundant sensing, 64 
References, 497 
reflection, 19, 20, 288 

composition with rotations, 45 
regional structure of a manipulator, 
106 

revolute pair, 104 
rheonomic systems, 398 
robotic hands, 9 

robotic mechanical system, viii, 3 
robotic system, 2 
Rodrigues (see Euler-Rodrigues) 
vector, 455 
rolling robots, 15 
dynamics, 413 
kinematics, 349 
rotations, 23 
rotation matrix, 28 

exponential representation, 30 
RVS, xiii, 189, 376 
run-time, 452 
Runge-Kutta method, 264 



scleronomic systems, 398 
screw 

amplitude, 72, 77, 83 
axis, 72, 77 

axis coordinates, 77, 80 
motion, 72 
pitch, 72, 83 
ray coordinates, 87 
self-inverse, 21 

semigraphical solution of the gene- 
ral IKP, 299 
serial manipulators, 6 
acceleration, 153 
dynamics, 209 
kinematics. 111 
statics, 158, 167 
velocity analysis, 135 
workspace, 150 
service angle, 168 
similarity transformations, 55 
simulation, 262 

singular- value decomposition, 170 
singular values, 170 
singularity analysis of decoupled 
manipulators, 147 
SPDM, 6 

spherical wrist, 106, 115, 132 
workspace, 132 
spline (s), 200 
natural, 205 
nonparametric, 381 
parametric, 381 
periodic, 201 

interpolation of 4-5-6-7 polyno- 
mial, 194 

square root of a matrix, 42 

Star robot, 9 

state 

of parallel manipulators, 409 
of serial manipulators, 232, 263 
variable, 213, 263 
-variable equations, 263 
-variable model of platform 
manipulators, 409 
vector, 263 
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static analysis 

of rigid bodies, 93 
of serial manipulators, 158, 167 
Stewart platform (see Stewart-Gough 
platform) 

Stewart-Gough platform, x, 323, 324 
direct kinematics, 326 
leg kinematics, 324 
structure of mechanical systems, 3 
structured environment, 3 
Sutherland, Sprout & Assoc. 

Hexapod, 13 
system, 1 



telemanipulators, 3 
tensors, 17, 211, 232 
Titan series of quadrapeds,13 
torsion, 363 

derivative w. r. t. the arc length, 
363 

parametric representation, 369, 
time-derivative, 366 
trace of a square matrix, 32 
trajectories with via poses, 199 
trajectory planning, 187, 361 
trapezoidal velocity profile, 474 
truncation error, 264 
Trussarm, 9, 10 
TU Munich Hand, 11 
TU Munich Hexapod, 14 
twist 

of a rigid body, 86 
transfer formula, 89 
twist-shape relations, 234 
for serial manipulators, 247 



vector of a 3 X 3 matrix, 32 
vector space, 18 
velocity analysis 

of parallel manipulators, 334 
of rolling robots, 351 
of serial manipulators, 135 
via poses, 199 

virtual work (see Principle of Virtual 
Work) 

viscosity coefficient, 267 
viscous forces, 243, 266 



walking machines, 13 
kinematics, 346 
leg architecture, 321, 324 
walking stick, 13 
workspace of positioning mani- 
pulators, 150 
wrench 

acting on a rigid body, 86 
axis, 94 
pitch, 94 

transfer formula, 96 



unimodular group (of matrices), 76 
unstructured environment, 3 
upper kinematic pair, 104 




